Merge branch 'master' of https://github.com/jsk-ros-pkg/jsk_recognition

jsk_pcl_ros/CMakeLists.txt

@@ -96,10 +96,13 @@ 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
 )
+rosbuild_add_openmp_flags(jsk_pcl_ros)
 
 rosbuild_add_compile_flags (jsk_pcl_ros ${SSE_FLAGS})
 rosbuild_link_boost (jsk_pcl_ros system)

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
@@ -39,6 +44,7 @@ generate_dynamic_reconfigure_options(
   cfg/MultiPlaneExtraction.cfg
   cfg/NormalEstimationIntegralImage.cfg
   cfg/PlaneRejector.cfg
+  cfg/OcludedPlaneEstimator.cfg
   )
 
 find_package(OpenCV REQUIRED core imgproc)
@@ -62,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
@@ -120,6 +126,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})

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"))

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"))

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 <ros/ros.h>
+#include <ros/names.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/synchronizer.h>
+#include <dynamic_reconfigure/server.h>
+
+// pcl
+#include <pcl_ros/pcl_nodelet.h>
+#include <pcl/point_types.h>
+
+#include <jsk_pcl_ros/PolygonArray.h>
+#include <jsk_pcl_ros/ModelCoefficientsArray.h>
+#include <jsk_pcl_ros/OcludedPlaneEstimatorConfig.h>
+
+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<jsk_pcl_ros::PolygonArray> sub_polygons_;
+    message_filters::Subscriber<jsk_pcl_ros::PolygonArray> sub_static_polygons_;
+    message_filters::Subscriber<jsk_pcl_ros::ModelCoefficientsArray> sub_coefficients_;
+    message_filters::Subscriber<jsk_pcl_ros::ModelCoefficientsArray> sub_static_coefficients_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
+    ros::Publisher polygon_pub_, coefficient_pub_;
+    boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
+    virtual void configCallback(Config &config, uint32_t level);
+    double plane_distance_threshold_;
+    double plane_angle_threshold_;
+  private:
+  };
+
+}
+
+#endif

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,30 +66,42 @@ 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 <dynamic_reconfigure::Server<Config> > srv_;
     boost::mutex mutex_;
     virtual void segment(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    virtual void estimateNormal(pcl::PointCloud<PointT>::Ptr input,
+                                pcl::PointCloud<pcl::Normal>::Ptr output);
     virtual void configCallback (Config &config, uint32_t level);
-    virtual void pointCloudToPolygon(const pcl::PointCloud<pcl::PointXYZRGBNormal>& input,
+    virtual void pointCloudToPolygon(const pcl::PointCloud<PointT>& input,
                                      geometry_msgs::Polygon& polygon);
     virtual void pclIndicesArrayToClusterPointIndices(const std::vector<pcl::PointIndices>& inlier_indices,
                                                       const std_msgs::Header& header,
                                                       jsk_pcl_ros::ClusterPointIndices& output_indices);
-    virtual void connectPlanesMap(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& input,
+    virtual void connectPlanesMap(const pcl::PointCloud<PointT>::Ptr& input,
                                   const std::vector<pcl::ModelCoefficients>& model_coefficients,
                                   const std::vector<pcl::PointIndices>& boundary_indices,
                                   std::vector<std::map<size_t, bool> >& connection_map);
-    virtual void buildConnectedPlanes(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& input,
+    virtual void buildConnectedPlanes(const pcl::PointCloud<PointT>::Ptr& input,
                                       const std_msgs::Header& header,
                                       const std::vector<pcl::PointIndices>& inlier_indices,
                                       const std::vector<pcl::PointIndices>& boundary_indices,
                                       const std::vector<pcl::ModelCoefficients>& model_coefficients,
                                       std::vector<std::map<size_t, bool> > connection_map,
                                       std::vector<pcl::PointIndices>& output_indices,
                                       std::vector<pcl::ModelCoefficients>& output_coefficients,
-                                      std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> >& output_boundary_clouds);
-
+                                      std::vector<pcl::PointCloud<PointT> >& output_boundary_clouds);
+
+    virtual void segmentFromNormals(pcl::PointCloud<PointT>::Ptr input,
+                                    pcl::PointCloud<pcl::Normal>::Ptr normal,
+                                    const std_msgs::Header& header);
   private:
     virtual void onInit();
   };

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<double>& result);
   private:
     virtual void onInit();
 

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,

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);

jsk_pcl_ros/jsk_pcl_nodelets.xml

@@ -1,4 +1,8 @@
 <library path="lib/libjsk_pcl_ros">
+  <class name="jsk_pcl/OcludedPlaneEstimator" type="OcludedPlaneEstimator"
+         base_class_type="nodelet::Nodelet">
+    <description>estimate the planes taking into account occulusions</description>
+  </class>
   <class name="jsk_pcl/PolygonArrayTransformer" type="PolygonArrayTransformer"
          base_class_type="nodelet::Nodelet">
     <description>transform polygons to the specified tf frame</description>

jsk_pcl_ros/launch/hrp2jsknt_footstep_polygon.launch

@@ -8,9 +8,11 @@
         output="screen"/>
   <node pkg="nodelet" type="nodelet" name="footstep_polygon_publisher"
         clear_params="true"
-        args="load jsk_pcl/StaticPolygonArrayPublisher /$(arg MANAGER)">
+        args="load jsk_pcl/StaticPolygonArrayPublisher /$(arg
+              MANAGER)">
+    <remap from="~input" to="/camera_remote/depth_registered/points" />
     <rosparam>
-      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]]]
     </rosparam>