table_objects.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: table_objects.cpp 30899 2010-07-16 04:56:51Z rusu $
00035  *
00036  */
00037 
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/PointCloud2.h>
00044 #include <mapping_msgs/CollisionObject.h>
00045 
00046 #include <pcl/point_types.h>
00047 #include <pcl/common/eigen.h>
00048 #include <pcl/features/feature.h>
00049 #include <pcl/segmentation/extract_clusters.h>
00050 
00051 #include <table_objects/GetObjects.h>
00052 #include <table_objects/GetLastObjects.h>
00053 
00054 #include <table_objects/table_objects.h>
00055 
00056 // Eigen
00057 #include <Eigen3/Core>
00058 #include <Eigen3/Geometry>
00059 
00060 using namespace pcl;
00061 
00062 bool classify;
00063 flann::Index< flann::L2<float> > *flann_index;
00064 
00065 /*
00066 #define MAX_QUEUE 20
00067 std::vector<mapping_msgs::CollisionObject> last_table_objects;
00068 
00069 bool
00070   getLastObjects (table_objects::GetLastObjects::Request &req, table_objects::GetLastObjects::Response &res)
00071 {
00072   std::vector<mapping_msgs::CollisionObject>::iterator oldest = last_table_objects.end ();
00073   if (req.number < last_table_objects.size ())
00074     oldest = last_table_objects.begin () + req.number;
00075   res.table_objects.insert (res.table_objects.end (), last_table_objects.begin (), oldest);
00076   return (true);
00077 }*/
00078 
00079 bool
00080   getTableObjects (table_objects::GetObjects::Request &req, table_objects::GetObjects::Response &res)
00081 {
00082   // ---[ Get the input
00083   PointCloud<PointXYZ> cloud;
00084   pcl::fromROSMsg (req.data, cloud);
00085 
00086   // ---[ Split the objects into Euclidean clusters
00087   std::vector<pcl::PointIndices> clusters;
00088   pcl::EuclideanClusterExtraction<PointXYZ> cluster;
00089   cluster.setInputCloud (cloud.makeShared ());
00090   cluster.setClusterTolerance (0.03);
00091   cluster.setMinClusterSize (20);
00092   cluster.extract (clusters);
00093   ROS_INFO ("[getTableObjects] Number of clusters found matching the given constraints: %d.", (int)clusters.size ());
00094 
00095   // ---[ Convert clusters to collision objects
00096   res.table_objects.reserve (clusters.size ());
00097   for (size_t i = 0; i < clusters.size (); ++i)
00098   {
00099     pcl::PointCloud<PointXYZ> cloud_object_cluster;
00100     pcl::copyPointCloud (cloud, clusters[i], cloud_object_cluster);
00101     std::stringstream ss;
00102     ss << "cluster_" << i;
00103     std::string label = ss.str ();
00104     
00105     // run VFH classification
00106     if (classify)
00107     {
00108       // compute normals
00109       pcl::PointCloud<pcl::Normal>::Ptr normals = boost::make_shared<pcl::PointCloud<pcl::Normal> >();
00110       pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n3d;
00111       n3d.setKSearch (30); // have to use the same parameters as during training
00112       pcl::KdTree<pcl::PointXYZ>::Ptr tree = boost::make_shared<pcl::KdTreeANN<pcl::PointXYZ> > ();
00113       n3d.setSearchMethod (tree);
00114       n3d.setInputCloud(cloud_object_cluster.makeShared ());
00115       n3d.compute(*normals);
00116       
00117       // compute vfh
00118       pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
00119       pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
00120       vfh.setSearchMethod (tree);
00121       vfh.setInputCloud(cloud_object_cluster.makeShared ());
00122       vfh.setInputNormals(normals);
00123       vfh.compute(vfh_signature);
00124       
00125       // search for nearest neighor
00126       int k = 1;
00127       flann::Matrix<float> p = flann::Matrix<float>(vfh_signature.points.at (0).histogram, 1, 308);
00128       flann::Matrix<int> k_indices = flann::Matrix<int>(new int[k], 1, k);
00129       flann::Matrix<float> k_distances = flann::Matrix<float>(new float[k], 1, k);
00130       flann_index->knnSearch (p, k_indices, k_distances, k, flann::SearchParams (512));
00131 
00132       // extracting the label name
00133       std::string vfh_model = models.at (k_indices[0][0]).first.c_str ();
00134       size_t pos1 = vfh_model.find_last_of ("/\\"); // just in case this will ever run on windows :P
00135       std::string vfh_label = vfh_model;
00136       if (pos1 != std::string::npos)
00137       {
00138         size_t pos2 = vfh_model.find ('.', pos1+1);
00139         vfh_label = vfh_model.substr (pos1+1, pos2-pos1-1);
00140       }
00141       while (vfh_label.find (".bag") != std::string::npos || vfh_label.find (".pcd") != std::string::npos)
00142         vfh_label = vfh_label.substr (0, vfh_label.size () - 4);
00143       
00144       // hard-code some nice label names for knowledge processing
00145       if (vfh_label.find ("cereal") != std::string::npos)
00146         label = "BreakfastCereal";
00147       else if (vfh_label.find ("milk") != std::string::npos)
00148         label = "CowsMilk-Product";
00149       else if (vfh_label.find ("lego") != std::string::npos)
00150         label = "Bowl-Eating"; // last minute change, as someone said bowls can not be grasped
00151       ROS_INFO ("VFH label for cluster %zu: %s", i, label.c_str ());
00152     }
00153     
00154     // compute 3D centroid
00155     Eigen3::Vector4f centroid;
00156     pcl::compute3DCentroid (cloud_object_cluster, centroid);
00157     
00158     // project to 2D
00159     pcl::PointCloud<PointXYZ> cloud_object_cluster_2D = cloud_object_cluster;
00160     for (size_t cp = 0; cp < cloud_object_cluster_2D.points.size (); cp ++)
00161       cloud_object_cluster_2D.points[cp].z /= 1000; // can't set it to 0 directly because of a bug in eigen33
00162       
00163     // get the rough oriented bounding box
00164     Eigen3::Vector4f centroid2D;
00165     pcl::compute3DCentroid (cloud_object_cluster_2D, centroid2D);
00166     Eigen3::Matrix3f covariance_matrix;
00167     computeCovarianceMatrix (cloud_object_cluster_2D, centroid2D, covariance_matrix);
00168     Eigen3::Matrix3f evecs;
00169     Eigen3::Vector3f evals;
00170     pcl::eigen33 (covariance_matrix, evecs, evals);
00171     //cerr << "Eigenvalues followed by eigenvectors: " << evals.transpose () << endl;
00172     //cerr << evecs << endl;
00173     Eigen3::Matrix3f rotation;
00174     rotation.row (2) = evecs.col (0);
00175     rotation.row (1) = evecs.col (1);
00176     rotation.row (0) = rotation.row (1).cross (rotation.row (2));
00177     //rotation.transposeInPlace ();
00178     cerr << "Rotation matrix: " << endl;
00179     cerr << rotation << endl;
00180     //cerr << "norms: " << rotation.row (0).norm () << " " << rotation.row (1).norm () << " " << rotation.row (2).norm () << endl;
00181     //cerr << "Quaternions: " << qt.x () << " " << qt.y () << " " << qt.z () << " " << qt.w () << endl;
00182     
00183     //pcl::copyPointCloud (cloud, clusters[i], cloud_object_cluster);
00184     Eigen3::Array3f min_point_projected (+FLT_MAX, +FLT_MAX, +FLT_MAX);
00185     Eigen3::Array3f max_point_projected (-FLT_MAX, -FLT_MAX, -FLT_MAX);
00186     Eigen3::Array3f min_point_base (+FLT_MAX, +FLT_MAX, +FLT_MAX);
00187     Eigen3::Array3f max_point_base (-FLT_MAX, -FLT_MAX, -FLT_MAX);
00188     for (size_t cp = 0; cp < cloud_object_cluster.points.size (); cp ++)
00189     {
00190       Eigen3::Map<Eigen3::Vector3f> point (&cloud_object_cluster.points[cp].x);
00191       Eigen3::Array3f transformed = rotation * (point - centroid.head<3> ());
00192       //cerr << point[2] << "/" << (point - centroid2D.head<3> ())[2] << "/" << transformed[0] << " ";
00193       
00194       min_point_base = min_point_base.min (point.array());
00195       max_point_base = max_point_base.max (point.array());
00196 
00197       min_point_projected = min_point_projected.min (transformed);
00198       max_point_projected = max_point_projected.max (transformed);
00199     }
00200     //cerr << "Minimum corner: " << min_point.transpose () << endl;
00201     //cerr << "Maximum corner: " << max_point.transpose () << endl;
00202     Eigen3::Array3f center_offset = min_point_base + (max_point_base - min_point_base)/2;
00203 
00204     
00205     rotation.transposeInPlace ();
00206     Eigen3::Quaternion<float> qt (rotation);
00207     qt.normalize ();
00208 
00209     // create the collison object
00210     mapping_msgs::CollisionObject collision_object;
00211     collision_object.header = cloud.header;
00212     collision_object.id = label;
00213     collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00214     collision_object.shapes.resize (1);
00215     collision_object.shapes[0].type = geometric_shapes_msgs::Shape::BOX;
00216     collision_object.shapes[0].dimensions.resize (3);
00217     collision_object.shapes[0].dimensions[0] = std::max (0.01f, max_point_projected[0] - min_point_projected[0]);
00218     collision_object.shapes[0].dimensions[1] = std::max (0.01f, max_point_projected[1] - min_point_projected[1]);
00219     collision_object.shapes[0].dimensions[2] = std::max (0.01f, max_point_projected[2] - min_point_projected[2]);
00220     collision_object.poses.resize (1);
00221     collision_object.poses[0].position.x = center_offset[0];
00222     collision_object.poses[0].position.y = center_offset[1];
00223     collision_object.poses[0].position.z = center_offset[2];
00224     collision_object.poses[0].orientation.x = qt.x ();
00225     collision_object.poses[0].orientation.y = qt.y ();
00226     collision_object.poses[0].orientation.z = qt.z ();
00227     collision_object.poses[0].orientation.w = qt.w ();
00228     res.table_objects.push_back (collision_object);
00229     
00230     /*// save the last MAX_QUEUE objects
00231     last_table_objects.insert (last_table_objects.begin (), collision_object);
00232     if (last_table_objects.size () > MAX_QUEUE)
00233       last_table_objects.resize (MAX_QUEUE);*/
00234   }
00235 
00236   return (true);
00237 }
00238 
00239 /* ---[ */
00240 int
00241   main (int argc, char** argv)
00242 {
00243   // never the case, only to have the parameter descriptions in the code
00244   if (argc < 1)
00245   {
00246     print_error ("Syntax is: %s [options] {kdtree.idx} {training_data.h5} {training_data.list}\n", argv[0]);
00247     print_info  ("    where options are:  -metric = metric/distance type:  1 = Euclidean, 2 = Manhattan, 3 = Minkowski, 4 = Max, 5 = HIK, 6 = JM, 7 = Chi-Square (default: "); print_value ("%d", metric); print_info (")\n\n");
00248     //
00249     print_info  ("      * note: the metric_type has to match the metric that was used when the tree was created.\n");
00250     print_info  ("              the last three parameters are optional and represent: the kdtree index file (default: "); print_value ("kdtree.idx"); print_info (")\n"
00251                  "                                                                    the training data used to create the tree (default: "); print_value ("training_data.h5"); print_info (")\n"
00252                  "                                                                    the list of models used in the training data (default: "); print_value ("training_data.list"); print_info (")\n");
00253     return (-1);
00254   }
00255 
00256   // Check if classification can be done and repare everything for it
00257   classify = false;
00258   if (getParameters (argc, argv) == 1)
00259   {
00260     classify = true;
00261     // Load trainin data into FLANN
00262     loadFileList (models, training_data_list_file_name);
00263     flann::Matrix<float> data;
00264     flann::load_from_file (data, training_data_h5_file_name, "training_data");
00265     print_highlight ("Training data found. Loaded %d VFH models from %s/%s.\n", (int)data.rows, training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());
00266     // Initialize FLAN indices
00267     flann_index = new flann::Index< flann::L2<float> > (data, flann::SavedIndexParams (kdtree_idx_file_name));
00268     flann_index->buildIndex ();
00269   }
00270   else
00271     ROS_WARN ("VFH classification is disabled!");
00272   
00273   //last_table_objects.reserve (MAX_QUEUE);
00274   
00275   ros::init (argc, argv, "table_objects");
00276 
00277   ros::NodeHandle nh;
00278 
00279   ros::ServiceServer service = nh.advertiseService ("get_table_objects", getTableObjects);
00280   //ros::ServiceServer service2 = nh.advertiseService ("get_last_objects", getLastObjects);
00281   
00282   ros::spin ();
00283 
00284   return (0);
00285 }
00286 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


table_objects
Author(s): Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 10:18:16