[jsk_pcl_ros_utils] add PolygonArrayLikelihoodFilter

doc/jsk_pcl_ros_utils/nodes/polygon_array_likelihood_filter.rst

@@ -0,0 +1,48 @@
+PolygonArrayLikelihoodFilter
+=====================
+
+.. image:: ./images/polygon_array_likelihood_filter.png
+
+Filter ``jsk_recognition_msgs/PolygonArray`` by likelihood.
+
+
+Subscribing Topics
+------------------
+
+* ``~input_polygons`` (``jsk_recognition_msgs/PolygonArray``)
+
+  Input polygon array.
+
+* ``~input_coefficients`` (``jsk_recognition_msgs/ModelCoefficientsArray``)
+
+  Input coefficients array.
+  (Enabled if ``use_coefficients`` is ``true``. Expected the same order with input polygons)
+
+
+Publishing Topics
+-----------------
+
+* ``~output_polygon`` (``jsk_recognition_msgs/PolygonArray``)
+
+  Filtered polygon array. (Polygons are sorted by their likelihood.)
+
+* ``~output_coefficients`` (``jsk_recognition_msgs/ModelCoefficientsArray``)
+
+  Filtered coefficients array. (Published only if ``use_coefficients`` is ``true``.)
+
+
+Parameters
+----------
+
+* ``~use_coefficients`` (Bool, default: ``true``)
+
+  If ``true``, polygons and coefficients are subscribed and published synchronously.
+
+* ``~threshold`` (Double, default: ``0.5``)
+
+  Threshold for filtering polygons.
+  See also description of ``~negative`` below for more detail.
+
+* ``~negative`` (Bool, default: ``false``)
+
+  If ``false``, published polygons whose likelihood is higher than ``~threshold``, lower otherwise.

jsk_pcl_ros_utils/CMakeLists.txt

@@ -81,6 +81,7 @@ generate_dynamic_reconfigure_options(
   cfg/PlaneRejector.cfg
   cfg/PointCloudToPCD.cfg
   cfg/PolygonArrayAreaLikelihood.cfg
+  cfg/PolygonArrayLikelihoodFilter.cfg
   cfg/PolygonArrayUnwrapper.cfg
   cfg/PolygonMagnifier.cfg
   cfg/PolygonPointsSampler.cfg
@@ -130,6 +131,8 @@ jsk_pcl_util_nodelet(src/polygon_array_angle_likelihood_nodelet.cpp
   "jsk_pcl_utils/PolygonArrayAngleLikelihood" "polygon_array_angle_likelihood")
 jsk_pcl_util_nodelet(src/polygon_array_foot_angle_likelihood_nodelet.cpp
   "jsk_pcl_utils/PolygonArrayFootAngleLikelihood" "polygon_array_foot_angle_likelihood")
+jsk_pcl_util_nodelet(src/polygon_array_likelihood_filter_nodelet.cpp
+  "jsk_pcl_utils/PolygonArrayLikelihoodFilter" "polygon_array_likelihood_filter")
 jsk_pcl_util_nodelet(src/pose_with_covariance_stamped_to_gaussian_pointcloud_nodelet.cpp
   "jsk_pcl_utils/PoseWithCovarianceStampedToGaussianPointCloud" "pose_with_covariance_stamped_to_gaussian_pointcloud")
 jsk_pcl_util_nodelet(src/spherical_pointcloud_simulator_nodelet.cpp
@@ -251,6 +254,7 @@ if (CATKIN_ENABLE_TESTING)
     add_rostest(test/evaluate_voxel_segmentation_by_gt_box.test)
     add_rostest(test/pointcloud_to_mask_image.test)
     add_rostest(test/polygon_array_unwrapper.test)
+    add_rostest(test/polygon_array_likelihood_filter.test)
     add_rostest(test/polygon_magnifier.test)
     add_rostest(test/subtract_point_indices.test)
     roslaunch_add_file_check(test/subtract_point_indices.test)

jsk_pcl_ros_utils/cfg/PolygonArrayLikelihoodFilter.cfg

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+PACKAGE = 'jsk_pcl_ros_utils'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#        variable  type  level  description  default  min  max
+gen.add("queue_size", int_t, 0, "queue size", 100, 1, 1000)
+gen.add("threshold", double_t, 0, "Minimum likelihood", 0.5, 0.0, 1.0)
+gen.add("negative", bool_t, 0, "Choose negative value", False)
+exit(gen.generate(PACKAGE, PACKAGE, "PolygonArrayLikelihoodFilter"))

jsk_pcl_ros_utils/include/jsk_pcl_ros_utils/polygon_array_likelihood_filter.h

@@ -0,0 +1,89 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, 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 JSK Lab 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_UTILS_POLYGON_ARRAY_LIKELIHOOD_FILTER_H_
+#define JSK_PCL_ROS_UTILS_POLYGON_ARRAY_LIKELIHOOD_FILTER_H_
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/synchronizer.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <jsk_pcl_ros_utils/PolygonArrayLikelihoodFilterConfig.h>
+#include <jsk_recognition_msgs/PolygonArray.h>
+#include <jsk_recognition_msgs/ModelCoefficientsArray.h>
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+
+
+namespace jsk_pcl_ros_utils
+{
+  class PolygonArrayLikelihoodFilter: public jsk_topic_tools::DiagnosticNodelet
+  {
+  public:
+    typedef PolygonArrayLikelihoodFilterConfig Config;
+    typedef message_filters::sync_policies::ExactTime<
+      jsk_recognition_msgs::PolygonArray,
+      jsk_recognition_msgs::ModelCoefficientsArray>
+    SyncPolicy;
+    PolygonArrayLikelihoodFilter(): DiagnosticNodelet("PolygonArrayLikelihoodFilter") {}
+  protected:
+    virtual void onInit();
+    virtual void subscribe();
+    virtual void unsubscribe();
+    virtual void configCallback(Config &config, uint32_t level);
+    virtual void filter(
+      const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons);
+    virtual void filter(
+      const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
+      const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients);
+    boost::mutex mutex_;
+    boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
+    message_filters::Subscriber<jsk_recognition_msgs::PolygonArray> sub_polygons_;
+    message_filters::Subscriber<jsk_recognition_msgs::ModelCoefficientsArray> sub_coefficients_;
+    ros::Subscriber sub_polygons_alone_;
+    ros::Publisher pub_polygons_;
+    ros::Publisher pub_coefficients_;
+    bool negative_;
+    bool use_coefficients_;
+    double threshold_;
+    size_t queue_size_;
+  private:
+
+  };
+}
+
+#endif

jsk_pcl_ros_utils/jsk_pcl_nodelets.xml

@@ -95,6 +95,10 @@
          type="jsk_pcl_ros_utils::PolygonArrayFootAngleLikelihood"
          base_class_type="nodelet::Nodelet">
   </class>
+  <class name="jsk_pcl_utils/PolygonArrayLikelihoodFilter"
+         type="jsk_pcl_ros_utils::PolygonArrayLikelihoodFilter"
+         base_class_type="nodelet::Nodelet">
+  </class>
   <class name="jsk_pcl_utils/PoseWithCovarianceStampedToGaussianPointCloud" 
          type="jsk_pcl_ros_utils::PoseWithCovarianceStampedToGaussianPointCloud"
          base_class_type="nodelet::Nodelet">

jsk_pcl_ros_utils/sample/sample_polygon_array_likelihood_filter.launch

@@ -0,0 +1,14 @@
+<launch>
+  <include file="$(find jsk_recognition_utils)/sample/sample_polygon_array_publisher.launch">
+    <arg name="gui" value="false" />
+  </include>
+
+  <node name="polygon_array_likelihood_filter"
+        pkg="jsk_pcl_ros_utils" type="polygon_array_likelihood_filter">
+    <remap from="~input_polygons" to="/polygon_array_publisher/output" />
+    <rosparam>
+      use_coefficients: false
+      threshold: 0.8
+    </rosparam>
+  </node>
+</launch>

jsk_pcl_ros_utils/src/polygon_array_likelihood_filter_nodelet.cpp

@@ -0,0 +1,166 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, 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 JSK Lab 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 <algorithm>
+#include <jsk_pcl_ros_utils/polygon_array_likelihood_filter.h>
+
+namespace jsk_pcl_ros_utils
+{
+  void PolygonArrayLikelihoodFilter::onInit()
+  {
+    DiagnosticNodelet::onInit();
+
+    srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
+    dynamic_reconfigure::Server<Config>::CallbackType f =
+      boost::bind(&PolygonArrayLikelihoodFilter::configCallback, this, _1, _2);
+    srv_->setCallback(f);
+
+    pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(
+      *pnh_, "output_polygons", 1);
+
+    pnh_->param<bool>("use_coefficients", use_coefficients_, true);
+    if (use_coefficients_) {
+      pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
+        *pnh_, "output_coefficients", 1);
+    }
+
+    onInitPostProcess();
+  }
+
+  void PolygonArrayLikelihoodFilter::subscribe()
+  {
+    if (use_coefficients_) {
+      sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
+      sub_polygons_.subscribe(*pnh_, "input_polygons", 1);
+      sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
+      sync_->connectInput(sub_polygons_, sub_coefficients_);
+      sync_->registerCallback(boost::bind(
+                                &PolygonArrayLikelihoodFilter::filter,
+                                this, _1, _2));
+    } else {
+      sub_polygons_alone_ = pnh_->subscribe(
+        "input_polygons", 1, &PolygonArrayLikelihoodFilter::filter, this);
+    }
+  }
+
+  void PolygonArrayLikelihoodFilter::unsubscribe()
+  {
+    if (use_coefficients_) {
+      sub_polygons_.unsubscribe();
+      sub_coefficients_.unsubscribe();
+    } else {
+      sub_polygons_alone_.shutdown();
+    }
+  }
+
+  void PolygonArrayLikelihoodFilter::configCallback(Config& config, uint32_t level)
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+    threshold_ = config.threshold;
+    negative_ = config.negative;
+    if (queue_size_ != config.queue_size) {
+      queue_size_ = config.queue_size;
+      unsubscribe();
+      subscribe();
+    }
+  }
+
+  void PolygonArrayLikelihoodFilter::filter(
+    const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
+  {
+    jsk_recognition_msgs::ModelCoefficientsArray::Ptr dummy;
+    dummy.reset();
+    filter(polygons, dummy);
+  }
+
+  void PolygonArrayLikelihoodFilter::filter(
+    const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
+    const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
+  {
+    boost::mutex::scoped_lock lock(mutex_);
+    if (polygons->polygons.size() != polygons->likelihood.size()) {
+      ROS_ERROR_STREAM_THROTTLE(1.0, "The size of polygons " << polygons->polygons.size()
+                                << " must be same as the size of likelihood "
+                                << polygons->likelihood.size());
+      return;
+    }
+    if (use_coefficients_ &&
+        polygons->polygons.size() != coefficients->coefficients.size()) {
+      ROS_ERROR_STREAM_THROTTLE(1.0, "The size of polygons " << polygons->polygons.size()
+                                << "must be same as the size of coeeficients "
+                                << coefficients->coefficients.size());
+      return;
+    }
+    vital_checker_->poke();
+
+    bool use_labels = polygons->polygons.size() == polygons->labels.size();
+
+    std::vector<std::pair<double, int> > lookup_table;
+    lookup_table.resize(polygons->polygons.size());
+    for (int i = 0; i < polygons->polygons.size(); ++i) {
+      lookup_table[i] = std::pair<double, int>(polygons->likelihood[i], i);
+    }
+    std::sort(lookup_table.rbegin(), lookup_table.rend());
+
+    jsk_recognition_msgs::PolygonArray ret_polygons;
+    jsk_recognition_msgs::ModelCoefficientsArray ret_coefficients;
+
+    for (int i = 0; i < lookup_table.size(); ++i) {
+      double likelihood = lookup_table[i].first;
+      int idx = lookup_table[i].second;
+      if ((!negative_ && likelihood >= threshold_) ||
+          (negative_  && likelihood <  threshold_)) {
+        ret_polygons.polygons.push_back(polygons->polygons[idx]);
+        ret_polygons.likelihood.push_back(polygons->likelihood[idx]);
+        if (use_labels) {
+          ret_polygons.labels.push_back(polygons->labels[idx]);
+        }
+        if (use_coefficients_) {
+          ret_coefficients.coefficients.push_back(coefficients->coefficients[idx]);
+        }
+      }
+    }
+
+    ret_polygons.header = polygons->header;
+    pub_polygons_.publish(ret_polygons);
+    if (use_coefficients_) {
+      ret_coefficients.header = coefficients->header;
+      pub_coefficients_.publish(ret_coefficients);
+    }
+  }
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PolygonArrayLikelihoodFilter, nodelet::Nodelet);

jsk_pcl_ros_utils/test/polygon_array_likelihood_filter.test

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find jsk_pcl_ros_utils)/sample/sample_polygon_array_likelihood_filter.launch"/>
+  <test test-name="test_polygon_array_likelihood_filter"
+        pkg="jsk_pcl_ros_utils" type="test_polygon_array_likelihood_filter.py" />
+</launch>