add organized point cloud

jsk_pcl_ros/CMakeLists.txt

@@ -106,6 +106,8 @@ jsk_pcl_nodelet(src/grid_sampler_nodelet.cpp
   "jsk_pcl/GridSampler" "grid_sampler")
 jsk_pcl_nodelet(src/handle_estimator_nodelet.cpp
   "jsk_pcl/HandleEstimator" "handle_estimator")
+jsk_pcl_nodelet(src/organize_pointcloud_nodelet.cpp
+  "jsk_pcl/OrganizePointCloud" "organize_pointcloud")
 
 rosbuild_add_library (jsk_pcl_ros
   ${jsk_pcl_nodelet_sources}

jsk_pcl_ros/catkin.cmake

@@ -159,6 +159,8 @@ jsk_pcl_nodelet(src/grid_sampler_nodelet.cpp
   "jsk_pcl/GridSampler" "grid_sampler")
 jsk_pcl_nodelet(src/handle_estimator_nodelet.cpp
   "jsk_pcl/HandleEstimator" "handle_estimator")
+jsk_pcl_nodelet(src/organize_pointcloud_nodelet.cpp
+  "jsk_pcl/OrganizePointCloud" "organize_pointcloud")
 
 add_library(jsk_pcl_ros SHARED ${jsk_pcl_nodelet_sources}
   src/grid_index.cpp src/grid_map.cpp src/grid_line.cpp src/geo_util.cpp

jsk_pcl_ros/include/jsk_pcl_ros/organize_pointcloud.h

@@ -0,0 +1,65 @@
+// -*- mode: C++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Ryohei Ueda and 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_ORGANIZE_POINTCLOUD_H_
+#define JSK_PCL_ROS_ORGANIZE_POINTCLOUD_H_
+
+// ros
+#include <ros/ros.h>
+#include <ros/names.h>
+#include <sensor_msgs/PointCloud2.h>
+
+// pcl
+#include <pcl_ros/pcl_nodelet.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/range_image/range_image.h>
+
+namespace jsk_pcl_ros
+{
+  class OrganizePointCloud: public pcl_ros::PCLNodelet
+  {
+  protected:
+    double angular_resolution, angle_width, angle_height;
+    int min_points;
+    ros::Subscriber sub_;
+    ros::Publisher pub_;
+    virtual void extract(const sensor_msgs::PointCloud2ConstPtr &input);
+  private:
+    virtual void onInit();
+  };
+}
+
+#endif

jsk_pcl_ros/jsk_pcl_nodelets.xml

@@ -218,4 +218,10 @@
       jsk_pcl_ros::ColorizeMapRandomForest
     </description>
   </class>
+  <class name="jsk_pcl/OrganizePointCloud" type="OrganizePointCloud"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      jsk_pcl_ros::OrganizePointCloud
+    </description>
+  </class>
 </library>

jsk_pcl_ros/src/organize_pointcloud_nodelet.cpp

@@ -0,0 +1,90 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Ryohei Ueda and 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/organize_pointcloud.h"
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/common/centroid.h>
+
+namespace jsk_pcl_ros
+{
+  void OrganizePointCloud::extract(const sensor_msgs::PointCloud2ConstPtr &input)
+  {
+    // skip empty cloud
+    ROS_INFO_STREAM("received input clouds, convert range image, resolution: " << angular_resolution << ", width(deg): " << angle_width << ", height(deg):" << angle_height << ", min_points:" << min_points);
+
+    if ( input->width < min_points ) return;
+
+    pcl::PointCloud<pcl::PointXYZ> pointCloud;
+    pcl::fromROSMsg(*input, pointCloud);
+
+    // We now want to create a range image from the above point cloud, with a 1deg angular resolution
+    float angularResolution = (float) (angular_resolution * (M_PI/180.0f));  //   1.0 degree in radians
+    float maxAngleWidth     = (float) (angle_width * (M_PI/180.0f));  // 120.0 degree in radians
+    float maxAngleHeight    = (float) (angle_height * (M_PI/180.0f));  // 90.0 degree in radians
+    Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
+    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
+    float noiseLevel=0.00;
+    float minRange = 0.0f;
+    int borderSize = 1;
+
+    pcl::RangeImage rangeImage;
+    rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
+				    sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
+    ROS_INFO_STREAM("input image size " << input->width << " x " << input->height << "(=" << input->width * input->height << ")");
+    ROS_INFO_STREAM("output image size " << rangeImage.width << " x " << rangeImage.height << "(=" << rangeImage.width * rangeImage.height << ")");
+    ROS_DEBUG_STREAM(rangeImage);
+
+    sensor_msgs::PointCloud2 out;
+    pcl::toROSMsg(rangeImage, out);
+    out.header = input->header;
+    pub_.publish(out);
+  }
+
+  void OrganizePointCloud::onInit(void)
+  {
+    PCLNodelet::onInit();
+    sub_ = pnh_->subscribe("input", 1, &OrganizePointCloud::extract, this);
+    pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("output", 1);
+    pnh_->param<double>("angular_resolution", angular_resolution, 1.0);
+    pnh_->param<double>("angle_width", angle_width, 120.0);
+    pnh_->param<double>("angle_height", angle_height, 90.0);
+    pnh_->param<int>("min_points", min_points, 1000);
+  }
+}
+
+typedef jsk_pcl_ros::OrganizePointCloud OrganizePointCloud;
+PLUGINLIB_DECLARE_CLASS (jsk_pcl, OrganizePointCloud, OrganizePointCloud, nodelet::Nodelet);
+