convex_hull.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: convex_hull.hpp 32993 2010-09-30 23:08:57Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/common/io.h>
00040 #include "pcl_ros/surface/convex_hull.h"
00041 #include <geometry_msgs/PolygonStamped.h>
00042 
00044 void
00045 pcl_ros::ConvexHull2D::onInit ()
00046 {
00047   ros::NodeHandle pnh = getMTPrivateNodeHandle ();
00048   pub_output_ = pnh.advertise<PointCloud> ("output", max_queue_size_);
00049   pub_plane_  = pnh.advertise<geometry_msgs::PolygonStamped>("output_polygon", max_queue_size_);
00050 
00051   // ---[ Optional parameters
00052   pnh.getParam ("use_indices", use_indices_);
00053 
00054   // If we're supposed to look for PointIndices (indices)
00055   if (use_indices_)
00056   {
00057     // Subscribe to the input using a filter
00058     sub_input_filter_.subscribe (pnh, "input", 1);
00059     // If indices are enabled, subscribe to the indices
00060     sub_indices_filter_.subscribe (pnh, "indices", 1);
00061 
00062     if (approximate_sync_)
00063     {
00064       sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointIndices> > >(max_queue_size_);
00065       // surface not enabled, connect the input-indices duo and register
00066       sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00067       sync_input_indices_a_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
00068     }
00069     else
00070     {
00071       sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointIndices> > >(max_queue_size_);
00072       // surface not enabled, connect the input-indices duo and register
00073       sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00074       sync_input_indices_e_->registerCallback (bind (&ConvexHull2D::input_indices_callback, this, _1, _2));
00075     }
00076   }
00077   else
00078     // Subscribe in an old fashion to input only (no filters)
00079     sub_input_ = pnh.subscribe<PointCloud> ("input", 1,  bind (&ConvexHull2D::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00080 
00081   NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00082                  " - use_indices    : %s",
00083                  getName ().c_str (), 
00084                  (use_indices_) ? "true" : "false");
00085 }
00086 
00088 void
00089   pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud, 
00090                                                  const PointIndicesConstPtr &indices)
00091 {
00092   // No subscribers, no work
00093   if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
00094     return;
00095 
00096   PointCloud output;
00097 
00098   // If cloud is given, check if it's valid
00099   if (!isValid (cloud))
00100   {
00101     NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00102     // Publish an empty message
00103     output.header = cloud->header;
00104     pub_output_.publish (output.makeShared ());
00105     return;
00106   }
00107   // If indices are given, check if they are valid
00108   if (indices && !isValid (indices, "indices"))
00109   {
00110     NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00111     // Publish an empty message
00112     output.header = cloud->header;
00113     pub_output_.publish (output.makeShared ());
00114     return;
00115   }
00116 
00118   if (indices)
00119     NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
00120                    "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00121                    "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00122                    getName ().c_str (),
00123                    cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
00124                    indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
00125   else
00126     NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
00127 
00128   // Reset the indices and surface pointers
00129   IndicesPtr indices_ptr;
00130   if (indices)
00131     indices_ptr.reset (new std::vector<int> (indices->indices));
00132 
00133   impl_.setInputCloud (cloud);
00134   impl_.setIndices (indices_ptr);
00135 
00136   // Estimate the feature
00137   impl_.reconstruct (output);
00138 
00139   // If more than 3 points are present, send a PolygonStamped hull too
00140   if (output.points.size () >= 3)
00141   {
00142     geometry_msgs::PolygonStamped poly;
00143     poly.header = output.header;
00144     poly.polygon.points.resize (output.points.size ());
00145     // Get three consecutive points (without copying)
00146     pcl::Vector4fMap O = output.points[1].getVector4fMap ();
00147     pcl::Vector4fMap B = output.points[0].getVector4fMap ();
00148     pcl::Vector4fMap A = output.points[2].getVector4fMap ();
00149     // Check the direction of points -- polygon must have CCW direction
00150     Eigen::Vector4f OA = A - O;
00151     Eigen::Vector4f OB = B - O;
00152     Eigen::Vector4f N = OA.cross3 (OB);
00153     double theta = N.dot (O);
00154     bool reversed = false;
00155     if (theta < (M_PI / 2.0))
00156       reversed = true;
00157     for (size_t i = 0; i < output.points.size (); ++i)
00158     {
00159       if (reversed)
00160       {
00161         size_t j = output.points.size () - i - 1;
00162         poly.polygon.points[i].x = output.points[j].x;
00163         poly.polygon.points[i].y = output.points[j].y;
00164         poly.polygon.points[i].z = output.points[j].z;
00165       }
00166       else
00167       {
00168         poly.polygon.points[i].x = output.points[i].x;
00169         poly.polygon.points[i].y = output.points[i].y;
00170         poly.polygon.points[i].z = output.points[i].z;
00171       }
00172     }
00173     pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
00174   }
00175   // Publish a Boost shared ptr const data
00176   output.header = cloud->header;
00177   pub_output_.publish (output.makeShared ());
00178 }
00179 
00180 typedef pcl_ros::ConvexHull2D ConvexHull2D;
00181 PLUGINLIB_DECLARE_CLASS (pcl, ConvexHull2D, ConvexHull2D, nodelet::Nodelet);
00182 


pcl_ros
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:22:22