[merge after #2846] option to use ClusterPointIndices for outlier removal

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,13 +10,29 @@ 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`)
 
   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
 ```

jsk_pcl_ros/CMakeLists.txt

@@ -656,6 +656,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()

jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h

@@ -42,6 +42,13 @@
 #include <jsk_topic_tools/diagnostic_nodelet.h>
 #include <jsk_topic_tools/counter.h>
 #include <dynamic_reconfigure/server.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/synchronizer.h>
+
+#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<int> getFilteredIndices(const pcl::PointCloud<pcl::PointXYZ>::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<sensor_msgs::PointCloud2> sub_cloud_;
+    message_filters::Subscriber<jsk_recognition_msgs::ClusterPointIndices> sub_cpi_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
+    boost::shared_ptr<message_filters::Synchronizer<ApproximateSyncPolicy> >async_;
+
     ros::Publisher pub_;
     boost::shared_ptr <dynamic_reconfigure::Server<Config> > 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_;

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: <Fixed 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: <Fixed 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

jsk_pcl_ros/sample/sample_realsense_tabletop_object_detector.launch

@@ -0,0 +1,38 @@
+<launch>
+  <arg name="gui" default="true" />
+
+  <arg name="camera_ns" default="camera" />
+  <arg name="manager" default="realsense_nodelet_manager" />
+  <arg name="launch_manager" default="true" />
+
+  <include file="$(find jsk_pcl_ros)/sample/include/play_rosbag_baxter_realsense_l515.xml">
+    <arg name="launch_manager" value="$(arg launch_manager)" />
+    <arg name="manager" value="$(arg manager)" />
+  </include>
+
+  <include file="$(find jsk_pcl_ros)/sample/tabletop_object_detector.launch">
+    <arg name="input" value="/$(arg camera_ns)/depth_registered/points" />
+    <arg name="manager" value="$(arg manager)" />
+    <arg name="sensor_frame" value="/$(arg camera_ns)_color_optical_frame" />
+    <arg name="launch_manager" value="false" />
+    <arg name="launch_tracking" default="false" />
+    <arg name="launch_openni" default="false" />
+    <arg name="launch_rviz" default="false" />
+    <arg name="resize" value="true" />
+    <arg name="resize_step" value="2" />
+    <arg name="outlier_removal" value="true" />
+    <arg name="approximate_sync" value="true" />
+    <arg name="plane_min_size" value="10000" />
+    <arg name="object_min_height" value="0.01" />
+    <arg name="object_max_height" value="1000.0" />
+    <arg name="align_boxes" value="true" />
+    <arg name="align_boxes_with_plane" value="false" />
+    <arg name="target_frame_id" value="base" />
+    <arg name="use_pca" value="true" />
+    <arg name="sort_by" value="cloud_size" />
+  </include>
+
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" if="$(arg gui)"
+        args="-d $(find jsk_pcl_ros)/sample/rviz/realsense_tabletop_object_detector.rviz" />
+
+</launch>