object_grabber.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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  */
00035 
00040 // ROS core
00041 #include <ros/ros.h>
00042 // Messages
00043 #include <sensor_msgs/PointCloud2.h>
00044 // PCL stuff
00045 #include <pcl/point_types.h>
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/ros/conversions.h>
00048 #include "pcl/sample_consensus/method_types.h"
00049 #include "pcl/sample_consensus/model_types.h"
00050 #include <pcl/segmentation/sac_segmentation.h>
00051 #include <pcl/filters/voxel_grid.h>
00052 #include <pcl/filters/passthrough.h>
00053 #include <pcl/filters/project_inliers.h>
00054 #include <pcl/surface/convex_hull.h>
00055 #include "pcl/filters/extract_indices.h"
00056 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00057 #include "pcl/common/common.h"
00058 #include <pcl/io/pcd_io.h>
00059 #include "pcl/segmentation/extract_clusters.h"
00060 #include <pcl/features/normal_3d.h>
00061 #include <pcl/common/angles.h>
00062 
00063 #include <pcl_ros/publisher.h>
00064 #include <pcl_ros/transforms.h>
00065 
00066 #include <tf/transform_broadcaster.h>
00067 #include <tf/transform_listener.h>
00068 #include <tf/message_filter.h>
00069 
00070 #include "kinect_cleanup/FilterObject.h"
00071 #include "kinect_cleanup/GrabObject.h"
00072 #include "kinect_cleanup/GetReleasedObject.h"
00073 
00074 //#define TEST
00075 
00076 typedef pcl::PointXYZRGB Point;
00077 typedef pcl::PointCloud<Point> PointCloud;
00078 typedef PointCloud::Ptr PointCloudPtr;
00079 typedef PointCloud::ConstPtr PointCloudConstPtr;
00080 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00081 
00083 class ObjectGrabber 
00084   {
00085   public:
00087     ObjectGrabber (const ros::NodeHandle &nh) : nh_ (nh), to_select_ (false)
00088     {
00089       nh_.param("sac_distance", sac_distance_, 0.03);
00090       nh_.param("z_min_limit", z_min_limit_, 0.8);
00091       nh_.param("z_max_limit", z_max_limit_, 1.9);
00092       nh_.param("max_iter", max_iter_, 500);
00093       nh_.param("normal_distance_weight", normal_distance_weight_, 0.1);
00094       nh_.param("eps_angle", eps_angle_, 15.0);
00095       nh_.param("seg_prob", seg_prob_, 0.99);
00096       nh_.param("normal_search_radius", normal_search_radius_, 0.05);
00097       nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00098       nh_.param("object_cluster_min_size", object_cluster_min_size_, 100);
00099       nh_.param("object_cluster_max_size", object_cluster_max_size_, 10000);
00100       nh_.param("k", k_, 10);
00101       nh_.param("base_link_head_tilt_link_angle", base_link_head_tilt_link_angle_, 0.277);
00102       nh_.param("min_table_inliers", min_table_inliers_, 100);
00103       nh_.param("cluster_min_height", cluster_min_height_, 0.01);
00104       nh_.param("cluster_max_height", cluster_max_height_, 0.4);
00105 
00106       hull_pub_.advertise (nh_, "/hull", 10);
00107       object_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("/moved_object", 10);
00108       object_service_ = nh_.advertiseService("/get_released_object", &ObjectGrabber::getObject, this);
00109       ROS_INFO ("[%s] Advertising service on: %s", getName ().c_str (), object_service_.getService ().c_str ());
00110       grab_service_ = nh_.advertiseService("/grab_object", &ObjectGrabber::grabDirection, this);
00111       ROS_INFO ("[%s] Advertising service on: %s", getName ().c_str (), object_service_.getService ().c_str ());
00112       point_cloud_sub_ = nh_.subscribe("input", 1, &ObjectGrabber::inputCallback, this);
00113       ROS_INFO ("[%s] Subscribed to topic: %s", getName ().c_str (), point_cloud_sub_.getTopic ().c_str ());
00114 
00115       object_released_ = false;
00116       vgrid_.setFilterFieldName ("z");
00117       vgrid_.setFilterLimits (z_min_limit_, z_max_limit_);
00118 
00119       seg_.setDistanceThreshold (sac_distance_);
00120       seg_.setMaxIterations (max_iter_);
00121       seg_.setNormalDistanceWeight (normal_distance_weight_);
00122       seg_.setOptimizeCoefficients (true);
00123       seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE); // TODO remove "base_link_head_tilt_link_angle" and use SACMODEL_NORMAL_PARALLEL_PLANE
00124       seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
00125       seg_.setMethodType (pcl::SAC_RANSAC);
00126       seg_.setProbability (seg_prob_);
00127 
00128       proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00129       clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00130       clusters_tree_->setEpsilon (1);
00131       normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
00132 
00133       n3d_.setKSearch (k_); // TODO use radius search
00134       n3d_.setSearchMethod (normals_tree_);
00135     }
00136 
00138     virtual ~ObjectGrabber ()
00139     {
00140       for (size_t i = 0; i < table_coeffs_.size (); ++i)
00141         delete table_coeffs_[i];
00142     }
00143 
00144   private:
00145 
00147     void
00148     inputCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_in)
00149     {
00150       if (output_cloud_.width != 0)
00151       {
00152         if (!object_released_)
00153         {
00154           output_cloud_.header.stamp = ros::Time::now();
00155           object_pub_.publish (output_cloud_);
00156           float *xzy = (float*)&output_cloud_.data[0];
00157           ROS_INFO("Republished object in frame %s (%g,%g,%g)", output_cloud_.header.frame_id.c_str(), xzy[0], xzy[1], xzy[2]);
00158         }
00159         return;
00160       }
00161 
00162       // If nothing is pointed at
00163       if (!to_select_)
00164         return;
00165       //      std::cerr << "to_select_" << std::endl;
00166       // If there is no TF data available
00167       ros::Time time = ros::Time::now();
00168       bool found_transform = tf_listener_.waitForTransform("right_hand", "openni_rgb_optical_frame", time, ros::Duration(1));
00169       if (!found_transform)
00170       {
00171         std::cerr << "no transform found" << std::endl;
00172         return;
00173       }
00174       tf::StampedTransform c2h_transform;
00175       tf_listener_.lookupTransform("right_hand", "openni_rgb_optical_frame", time, c2h_transform);
00176       to_select_ = false;
00177 
00178       ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: cloud time " << cloud_in->header.stamp);
00179 
00180       // Filter the input dataser
00181       PointCloud cloud_raw, cloud;
00182       pcl::fromROSMsg (*cloud_in, cloud_raw);
00183       vgrid_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00184       vgrid_.filter (cloud);
00185 
00186       // Fit a plane (the table)
00187       pcl::ModelCoefficients table_coeff;
00188       pcl::PointIndices table_inliers;
00189       PointCloud cloud_projected;
00190       pcl::PointCloud<Point> cloud_hull;
00191 
00192       // ---[ Estimate the point normals
00193       pcl::PointCloud<pcl::Normal> cloud_normals;
00194       n3d_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00195       n3d_.compute (cloud_normals);
00196       cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));
00197 
00198       // TODO fabs? ... use parallel to Z.cross(X)!
00199 
00200       //z axis in Kinect frame
00201       btVector3 axis(0.0, 0.0, 1.0);
00202       //rotate axis around x in Kinect frame for an angle between base_link and head_tilt_link + 90deg
00203       //todo: get angle automatically
00204       btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), btScalar(base_link_head_tilt_link_angle_ + pcl::deg2rad(90.0)));
00205       //std::cerr << "axis: " << fabs(axis2.getX()) << " " << fabs(axis2.getY()) << " " << fabs(axis2.getZ()) << std::endl;
00206 
00207       seg_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00208       seg_.setInputNormals (cloud_normals_);
00209       seg_.setAxis (Eigen::Vector3f(fabs(axis2.getX()), fabs(axis2.getY()), fabs(axis2.getZ())));
00210       // seg_.setIndices (boost::make_shared<pcl::PointIndices> (selection));
00211       seg_.segment (table_inliers, table_coeff);
00212       ROS_INFO ("[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (),
00213                 table_coeff.values[0], table_coeff.values[1], table_coeff.values[2], table_coeff.values[3], (int)table_inliers.indices.size ());
00214       if ((int)table_inliers.indices.size () <= min_table_inliers_)
00215       {
00216         ROS_ERROR ("table has to few inliers");
00217         return;
00218       }
00219 
00220       // Project the table inliers using the planar model coefficients
00221       proj_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00222       proj_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
00223       proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (table_coeff));
00224       proj_.filter (cloud_projected);
00225       //cloud_pub_.publish (cloud_projected);
00226 
00227       // Create a Convex Hull representation of the projected inliers
00228       chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
00229       chull_.reconstruct (cloud_hull);
00230       ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull.points.size ());
00231       hull_pub_.publish (cloud_hull);
00232 
00233       // //Compute the area of the plane
00234       // std::vector<double> plane_normal(3);
00235       // plane_normal[0] = table_coeff.values[0];
00236       // plane_normal[1] = table_coeff.values[1];
00237       // plane_normal[2] = table_coeff.values[2];
00238       // area_ = compute2DPolygonalArea (cloud_hull, plane_normal);
00239       // //ROS_INFO("[%s] Plane area: %f", getName ().c_str (), area_);
00240       // if (area_ > (expected_table_area_ + 0.01))
00241       // {
00242       //   extract_.setInputCloud (boost::make_shared<PointCloud> (cloud));
00243       //   extract_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
00244       //   extract_.setNegative (true);
00245       //   pcl::PointCloud<Point> cloud_extracted;
00246       //   extract_.filter (cloud_extracted);
00247       //   //cloud_extracted_pub_.publish (cloud_extracted);
00248       //   cloud = cloud_extracted;
00249       // }
00250       //end while
00251       //ROS_INFO ("[%s] Publishing convex hull with: %d data points and area %lf.", getName ().c_str (), (int)cloud_hull.points.size (), area_);
00252       //cloud_pub_.publish (cloud_hull);
00253 
00254       // pcl::PointXYZRGB point_min;
00255       // pcl::PointXYZRGB point_max;
00256       // pcl::PointXYZ point_center;
00257       // pcl::getMinMax3D (cloud_hull, point_min, point_max);
00258       // //Calculate the centroid of the hull
00259       // point_center.x = (point_max.x + point_min.x)/2;
00260       // point_center.y = (point_max.y + point_min.y)/2;
00261       // point_center.z = (point_max.z + point_min.z)/2;
00262 
00263       // tf::Transform transform;
00264       // transform.setOrigin( tf::Vector3(point_center.x, point_center.y, point_center.z));
00265       // transform.setRotation( tf::Quaternion(0, 0, 0) );
00266       // transform_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00267       //                                                           cloud_raw.header.frame_id, rot_table_frame_));
00268 
00269       // ---[ Get the objects on top of the table - from the raw cloud!
00270       pcl::PointIndices cloud_object_indices;
00271       prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
00272       prism_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00273       prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
00274       prism_.segment (cloud_object_indices);
00275       //ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());
00276 
00277       pcl::PointCloud<Point> cloud_object;
00278       pcl::ExtractIndices<Point> extract_object_indices;
00279       //extract_object_indices.setInputCloud (cloud_all_minus_table_ptr);
00280       extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
00281       extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
00282       extract_object_indices.filter (cloud_object);
00283       //ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), (int)cloud_objects.points.size ());
00284 
00285       std::vector<pcl::PointIndices> clusters;
00286       cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_object));
00287       cluster_.setClusterTolerance (object_cluster_tolerance_);
00288       cluster_.setMinClusterSize (object_cluster_min_size_);
00289       cluster_.setMaxClusterSize (object_cluster_max_size_);
00290       cluster_.setSearchMethod (clusters_tree_);
00291       cluster_.extract (clusters);
00292 
00293       // TODO take closest to ray
00294       for (size_t i = 0; i < clusters.size (); i++)
00295       {
00296         // compute center and see if it is close enough to ray
00297         int cluster_center = clusters[i].indices[clusters[i].indices.size () / 2];
00298         Eigen::Vector4f pt = Eigen::Vector4f (cloud_object.points[cluster_center].x, cloud_object.points[cluster_center].y, cloud_object.points[cluster_center].z, 0);
00299         Eigen::Vector4f c = line_dir_.cross3 (line_point_ - pt); c[3] = 0;
00300 #ifndef TEST
00301         if (c.squaredNorm () / line_dir_squaredNorm_ > 0.25*0.25) // further then 10cm
00302           continue;
00303 #endif
00304         std::cerr << "found cluster" << std::endl;
00305         // TODO make more optimal? + ERRORS/INCONSISTENCY IN PCL: second formula and in c computation!
00306 
00307         //tf::transformPoint();
00308         //Eigen::Matrix4f transform_matrix;
00309         //pcl_ros::transformAsMatrix (c2h_transform, transform_matrix);
00310 
00311         // broadcast transform
00312 //        tf::Transform fixed_transform;
00313 //        fixed_transform.setOrigin (tf::Vector3(0.0, 0.0, 0.1));
00314 //        fixed_transform.setRotation (tf::Quaternion(0, 0, 0));
00315 //        transform_broadcaster_.sendTransform(tf::StampedTransform(fixed_transform, ros::Time::now(), "right_hand", "object_frame"));
00316 
00317         // transform object into right_hand frame
00318         pcl::PointCloud<Point> cloud_object_clustered ;
00319         pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
00320 //        Eigen::Matrix4f eigen_transform;
00321 //        pcl_ros::transformAsMatrix (c2h_transform, eigen_transform);
00322 //        pcl::transformPointCloud(cloud_object_clustered, output_cloud_, eigen_transform);
00323 //        output_cloud_.header.frame_id = ""right_hand";
00324         sensor_msgs::PointCloud2 cluster;
00325         pcl::toROSMsg (cloud_object_clustered, cluster);
00326 //        template <typename PointT> void transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform);
00327 //        void transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out);
00328         pcl_ros::transformPointCloud("right_hand", (tf::Transform)c2h_transform, cluster, output_cloud_);
00329         output_cloud_.header.frame_id = "right_hand"; // TODO needed?
00330 //        for (unsigned cp = 0; cp < output_cloud_.points.size (); cp++)
00331 //        {
00332 //          output_cloud_.points[i].x -= output_cloud_.points[0].x;
00333 //          output_cloud_.points[i].y -= output_cloud_.points[0].y;
00334 //          output_cloud_.points[i].z -= output_cloud_.points[0].z;
00335 //        }
00336 //        cloud_objects_pub_.publish (output_cloud_);
00337         object_pub_.publish (output_cloud_);
00338         ROS_INFO("Published object with %d points", (int)clusters[i].indices.size ());
00339 
00340         // TODO get a nicer rectangle around the object
00341         unsigned center_idx = cloud_object_indices.indices.at (cluster_center);
00342         unsigned row = center_idx / 640;
00343         unsigned col = center_idx - row * 640;
00344         ros::ServiceClient client = nh_.serviceClient<kinect_cleanup::FilterObject>("/filter_object");
00345         kinect_cleanup::FilterObject srv;
00346         srv.request.min_row = std::max (0, (int)row-50);
00347         srv.request.min_col = std::max (0, (int)col-40);
00348         srv.request.max_row = std::min (479, (int)row+50);
00349         srv.request.max_col = std::min (479, (int)col+40);
00350         srv.request.rgb = cloud_raw.points[640 * srv.request.max_row + srv.request.max_col].rgb;
00351         for (int i=0; i<4; i++)
00352           srv.request.plane_normal[i] = table_coeff.values[i];
00353         if (client.call(srv))
00354           ROS_INFO("Object filter service responded: %s", srv.response.error.c_str ());
00355         else
00356           ROS_ERROR("Failed to call object filter service!");
00357 
00358         // take only 1 cluster into account
00359         break;
00360       }
00361     }
00362 
00364     bool grabDirection (kinect_cleanup::GrabObject::Request  &req,
00365                         kinect_cleanup::GrabObject::Response &res)
00366     {
00367       if (to_select_)
00368       {
00369         res.error = "already grabbed";
00370         return true;
00371       }
00372       to_select_ = true;
00373       line_point_ = Eigen::Vector4f (req.point_line[0], req.point_line[1], req.point_line[2], 0);
00374       line_dir_ = Eigen::Vector4f (req.point_line[3], req.point_line[4], req.point_line[5], 0);
00375       line_dir_squaredNorm_ = line_dir_.squaredNorm ();
00376       res.error = "grabbing enabled";
00377       std::cerr << "point: " << line_point_.transpose() << " direction: " << line_dir_.transpose() <<  std::endl;
00378       return true;
00379     }
00380 
00382     bool getObject (kinect_cleanup::GetReleasedObject::Request  &req,
00383                     kinect_cleanup::GetReleasedObject::Response &res)
00384     {
00385       //pcl::toROSMsg (output_cloud_, res.object);
00386       object_released_ = req.object_released;
00387       res.object = output_cloud_;
00388       return true;
00389     }
00390 
00392 
00396     /*double
00397     compute2DPolygonalArea (PointCloud &points, const std::vector<double> &normal)
00398     {
00399       int k0, k1, k2;
00400       
00401       // Find axis with largest normal component and project onto perpendicular plane
00402       k0 = (fabs (normal.at (0) ) > fabs (normal.at (1))) ? 0  : 1;
00403       k0 = (fabs (normal.at (k0)) > fabs (normal.at (2))) ? k0 : 2;
00404       k1 = (k0 + 1) % 3;
00405       k2 = (k0 + 2) % 3;
00406 
00407       // cos(theta), where theta is the angle between the polygon and the projected plane
00408       double ct = fabs ( normal.at (k0) );
00409 
00410       double area = 0;
00411       float p_i[3], p_j[3];
00412 
00413       for (unsigned int i = 0; i < points.points.size (); i++)
00414         {
00415           p_i[0] = points.points[i].x; p_i[1] = points.points[i].y; p_i[2] = points.points[i].z;
00416           int j = (i + 1) % points.points.size ();
00417           p_j[0] = points.points[j].x; p_j[1] = points.points[j].y; p_j[2] = points.points[j].z;
00418 
00419           area += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
00420         }
00421       area = fabs (area) / (2 * ct);
00422 
00423       return (area);
00424     }*/
00425 
00426     // ROS stuff
00427     ros::NodeHandle nh_;
00428     tf::TransformBroadcaster transform_broadcaster_;
00429     tf::TransformListener tf_listener_;
00430     ros::Subscriber point_cloud_sub_;
00431     pcl_ros::Publisher<Point> hull_pub_;
00432     ros::Publisher object_pub_;
00433     ros::ServiceServer object_service_;
00434     ros::ServiceServer grab_service_;
00435 
00436     // Switch to enable segmenting
00437     bool to_select_, object_released_;
00438     Eigen::Vector4f line_point_;
00439     Eigen::Vector4f line_dir_;
00440     float line_dir_squaredNorm_;
00441 
00442     // Parameters
00443     double object_cluster_tolerance_, cluster_min_height_, cluster_max_height_;
00444     int object_cluster_min_size_, object_cluster_max_size_;
00445     double sac_distance_, normal_distance_weight_, z_min_limit_, z_max_limit_;
00446     double eps_angle_, seg_prob_, base_link_head_tilt_link_angle_;
00447     int k_, max_iter_, min_table_inliers_;
00448     double normal_search_radius_;
00449     
00450     // PCL objects
00451     pcl::PassThrough<Point> vgrid_;                   // Filtering object
00452     pcl::NormalEstimation<Point, pcl::Normal> n3d_;   //Normal estimation
00453     pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;               // Planar segmentation object
00454     pcl::ProjectInliers<Point> proj_;               // Inlier projection object
00455     pcl::ExtractIndices<Point> extract_;            // Extract (too) big tables
00456     pcl::ConvexHull<Point> chull_;
00457     pcl::ExtractPolygonalPrismData<Point> prism_;
00458     pcl::PointCloud<Point> cloud_objects_;
00459     pcl::EuclideanClusterExtraction<Point> cluster_;
00460     KdTreePtr clusters_tree_, normals_tree_;
00461 
00462     // Grabbed object in right_hand frame
00463     //pcl::PointCloud<Point> output_cloud_;
00464     sensor_msgs::PointCloud2 output_cloud_;
00465   
00466     // TODO why are these saved as fields?
00467     std::vector<Eigen::Vector4d *> table_coeffs_;
00468     pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_; // The resultant estimated point cloud normals for \a cloud_filtered_
00469 
00471 
00472     std::string getName () const { return ("ObjectGrabber"); }
00473 };
00474 
00475 /* ---[ */
00476 int
00477 main (int argc, char** argv)
00478 {
00479   ros::init (argc, argv, "object_grabber");
00480   ros::NodeHandle nh("~");
00481   ObjectGrabber ptu_calibrator (nh);
00482   ros::spin ();
00483 
00484   return 0;
00485 }
00486 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


kinect_cleanup
Author(s): Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Sun Oct 6 2013 11:57:20