tabletop_object_recognition.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
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 the Willow Garage 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 // Author(s): Marius Muja and Matei Ciocarlie
00036 
00037 #include <string>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <sensor_msgs/PointCloud.h>
00042 
00043 #include <visualization_msgs/Marker.h>
00044 
00045 #include <tf/transform_listener.h>
00046 
00047 #include <household_objects_database_msgs/DatabaseModelPose.h>
00048 #include <household_objects_database_msgs/DatabaseModelPoseList.h>
00049 #include <household_objects_database_msgs/GetModelMesh.h>
00050 
00051 #include "tabletop_object_detector/exhaustive_fit_detector.h"
00052 #include "tabletop_object_detector/marker_generator.h"
00053 #include "tabletop_object_detector/iterative_distance_fitter.h"
00054 #include "tabletop_object_detector/Table.h"
00055 #include "tabletop_object_detector/TabletopObjectRecognition.h"
00056 #include "tabletop_object_detector/ClearExclusionsList.h"
00057 #include "tabletop_object_detector/AddModelExclusion.h"
00058 #include "tabletop_object_detector/NegateExclusions.h"
00059 
00060 namespace tabletop_object_detector {
00061 
00062 class TabletopObjectRecognizer 
00063 {
00064 private:
00066   ros::NodeHandle nh_;
00068   ros::NodeHandle priv_nh_;
00070   ros::Subscriber cloud_new_sub_;
00072   ros::Publisher marker_pub_;
00074   ros::ServiceServer object_recognition_srv_;
00075 
00077   ros::ServiceServer add_model_exclusion_srv_;
00078 
00080   ros::ServiceServer clear_model_exclusions_srv_;
00082   ros::ServiceServer negate_exclusions_srv_;
00084   ros::ServiceClient get_model_mesh_srv_;
00085 
00087   double min_marker_quality_;
00089   int num_markers_published_;
00091   int current_marker_id_;
00092 
00094   ExhaustiveFitDetector<IterativeTranslationFitter> detector_;
00095 
00097   std::string model_set_;
00098 
00100   double fit_merge_threshold_;
00101 
00103   tf::TransformListener listener_;
00104 
00105   //------------------ Callbacks -------------------
00106 
00108   bool serviceCallback(TabletopObjectRecognition::Request &request, TabletopObjectRecognition::Response &response);
00109   bool addExclusionCB(AddModelExclusion::Request &request, AddModelExclusion::Response &response);
00110   bool clearExclusionsCB(ClearExclusionsList::Request &request, ClearExclusionsList::Response &response);
00111   bool negateExclusionsCB(NegateExclusions::Request &request, NegateExclusions::Response &response);
00112 
00114   template <class PointCloudType>
00115   void objectDetection(std::vector<PointCloudType> &clusters, int num_models,
00116                        const Table &table, bool perform_fit_merge,
00117                        TabletopObjectRecognition::Response &response);
00118 
00119   //-------------------- Misc -------------------
00120 
00122   double fitDistance(const ModelFitInfo &m1, const ModelFitInfo &m2);
00123 
00125   template <class PointCloudType>
00126   double fitClusterDistance(const ModelFitInfo &m, const PointCloudType &cluster);
00127 
00129   void publishFitMarkers(
00130                  const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models,
00131                  const Table &table);
00132 
00134   void clearOldMarkers(std::string frame_id);
00135 
00136 public:
00138   TabletopObjectRecognizer(ros::NodeHandle nh);
00139 
00141   ~TabletopObjectRecognizer() {}
00142 };
00143 
00144 TabletopObjectRecognizer::TabletopObjectRecognizer(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
00145 {
00146   num_markers_published_ = 1;
00147   current_marker_id_ = 1;
00148   
00149   marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("markers_out"), 10);
00150   
00151   std::string get_model_mesh_srv_name;
00152   priv_nh_.param<std::string>("get_model_mesh_srv", get_model_mesh_srv_name, "get_model_mesh_srv");
00153   while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && nh_.ok() ) 
00154   {
00155     ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
00156   }
00157   if (!nh_.ok()) exit(0);
00158   get_model_mesh_srv_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
00159     (get_model_mesh_srv_name, true);
00160   //ask fitter to load models from database
00161   priv_nh_.param<std::string>("model_set", model_set_, "");
00162   detector_.loadDatabaseModels(model_set_);
00163   
00164   object_recognition_srv_ = nh_.advertiseService(nh_.resolveName("object_recognition_srv"), 
00165                                                  &TabletopObjectRecognizer::serviceCallback, this);
00166   clear_model_exclusions_srv_ = priv_nh_.advertiseService("clear_exclusions_srv",
00167                                                           &TabletopObjectRecognizer::clearExclusionsCB, this);
00168   add_model_exclusion_srv_ = priv_nh_.advertiseService("add_exclusion_srv",
00169                                                        &TabletopObjectRecognizer::addExclusionCB, this);    
00170   negate_exclusions_srv_ = priv_nh_.advertiseService("negate_exclusions_srv",
00171                                                      &TabletopObjectRecognizer::negateExclusionsCB, this);
00172   
00173   //initialize operational flags
00174   priv_nh_.param<double>("fit_merge_threshold", fit_merge_threshold_, 0.05);
00175   priv_nh_.param<double>("min_marker_quality", min_marker_quality_, 0.003);
00176 }
00177 
00178 bool TabletopObjectRecognizer::clearExclusionsCB(ClearExclusionsList::Request &request, 
00179                                                  ClearExclusionsList::Response &response)
00180 {
00181   ROS_INFO("Clearing exclusions list");
00182   detector_.clearExclusionList();
00183   return true;
00184 }
00185 
00186 bool TabletopObjectRecognizer::addExclusionCB(AddModelExclusion::Request &request, 
00187                                               AddModelExclusion::Response &response)
00188 {
00189   ROS_INFO("Adding %d to exclusions list", request.model_id);
00190   detector_.addModelToExclusionList(request.model_id);
00191   return true;
00192 }
00193 
00194 bool TabletopObjectRecognizer::negateExclusionsCB(NegateExclusions::Request &request, 
00195                                                   NegateExclusions::Response &response)
00196 {
00197   ROS_INFO("Setting exclusions negation to %s", request.negate ? "true" : "false");
00198   detector_.setNegateExclusions(request.negate);
00199   return true;
00200 }
00201 
00204 bool TabletopObjectRecognizer::serviceCallback(TabletopObjectRecognition::Request &request, 
00205                                                TabletopObjectRecognition::Response &response)
00206 {
00207   //convert point clouds to table frame
00208   tf::Transform table_trans;
00209   tf::poseMsgToTF(request.table.pose.pose, table_trans);
00210   tf::StampedTransform table_trans_stamped(table_trans, request.table.pose.header.stamp, 
00211                                            request.table.pose.header.frame_id, "table_frame");
00212   tf::TransformListener listener;
00213   listener.setTransform(table_trans_stamped);
00214   for (size_t i = 0; i < request.clusters.size (); ++i)
00215   {
00216     if (request.clusters[i].header.frame_id != request.table.pose.header.frame_id)
00217     {
00218       ROS_ERROR("Recognition node requires all clusters to be in the same frame as the table");
00219       return false;
00220     }
00221     request.clusters[i].header.stamp = request.table.pose.header.stamp;
00222     try
00223     {
00224       listener.transformPointCloud("table_frame", request.clusters[i], request.clusters[i]); 
00225     }
00226     catch (tf::TransformException ex)
00227     {
00228       ROS_ERROR("Recognition node failed to transform cluster from frame %s into table frame; Exception: %s", 
00229                 request.clusters[i].header.frame_id.c_str(), ex.what());
00230       return false;
00231     }
00232   }
00233   ROS_DEBUG("Clusters converted to table frame");
00234 
00235   //run detection
00236   objectDetection<sensor_msgs::PointCloud>(request.clusters, request.num_models, request.table, request.perform_fit_merge, response);
00237 
00238   publishFitMarkers(response.models, request.table);
00239   clearOldMarkers(request.table.pose.header.frame_id);
00240 
00241   return true;
00242 }
00243 
00246 template <class PointCloudType>
00247 void TabletopObjectRecognizer::objectDetection(std::vector<PointCloudType> &clusters, 
00248                                                int num_models,
00249                                                const Table &table,
00250                                                bool perform_fit_merge,
00251                                                TabletopObjectRecognition::Response &response)
00252 {
00253   //do the model fitting part
00254   ROS_INFO("Fitting models to clusters");
00255   std::vector< std::vector<ModelFitInfo> > raw_fit_results;
00256   response.cluster_model_indices.resize( clusters.size(), -1 );
00257   for (size_t i=0; i<clusters.size(); i++) 
00258   {
00259     raw_fit_results.push_back( detector_.fitBestModels<PointCloudType>(clusters[i], std::max(1,num_models)) );
00260     response.cluster_model_indices[i] = i;
00261   }
00262   ROS_DEBUG("Raw fit results computed");
00263 
00264   //merge models that were fit very close to each other
00265   if (perform_fit_merge)
00266   {
00267     size_t i=0;
00268     while (i < clusters.size())
00269     {
00270       //if cluster i has already been merged continue
00271       if (response.cluster_model_indices[i] != (int)i || raw_fit_results.at(i).empty()) 
00272       {
00273         i++;
00274         continue;
00275       }
00276 
00277       size_t j;
00278       for (j=i+1; j<clusters.size(); j++)
00279       {
00280         //if cluster j has already been merged continue
00281         if (response.cluster_model_indices[j] != (int)j) continue;
00282         //if there are no fits, merge based on cluster vs. fit
00283         if (raw_fit_results.at(j).empty()) 
00284         {
00285           if ( fitClusterDistance<PointCloudType>( raw_fit_results.at(i).at(0), 
00286                                                    clusters.at(j) ) < fit_merge_threshold_ ) break;
00287           else continue;
00288         }
00289         //else merge based on fits
00290         if ( fitDistance(raw_fit_results.at(i).at(0), raw_fit_results.at(j).at(0)) < fit_merge_threshold_) break;
00291       }
00292       if (j<clusters.size())
00293       {
00294         ROS_DEBUG("Post-fit merging of clusters %u and %u", (unsigned int) i, (unsigned int) j);
00295         //merge cluster j into i
00296         clusters[i].points.insert( clusters[i].points.end(), clusters[j].points.begin(), clusters[j].points.end() );
00297         //delete fits for cluster j so we ignore it from now on
00298         raw_fit_results.at(j).clear();
00299         //fits for cluster j now point at fit for cluster i
00300         response.cluster_model_indices[j] = i;
00301         //refit cluster i
00302         raw_fit_results.at(i) = detector_.fitBestModels(clusters[i], std::max(1,num_models));
00303       }
00304       else
00305       {
00306         i++;
00307       }
00308     }
00309   }
00310   ROS_DEBUG("Post-fit merge completed");
00311 
00312   //make sure raw clusters point at the right index in fit_models
00313   for (size_t i=0; i<raw_fit_results.size(); i++)
00314   {
00315     if (response.cluster_model_indices[i] != (int)i)
00316     {
00317       int ind = response.cluster_model_indices[i];
00318       ROS_ASSERT( ind < (int)i);
00319       response.cluster_model_indices[i] = response.cluster_model_indices[ind];
00320       ROS_DEBUG("Fit for cluster %u has been merged with fit for cluster %u", (unsigned int) i, (unsigned int) ind);
00321     }
00322   }
00323   ROS_DEBUG("Clustered indices arranged");
00324 
00325   tf::Transform table_trans;
00326   tf::poseMsgToTF(table.pose.pose, table_trans);
00327   for (size_t i=0; i<raw_fit_results.size(); i++)
00328   {
00329     ROS_DEBUG("Cluster %u results:", (unsigned int) i);
00330     household_objects_database_msgs::DatabaseModelPoseList model_potential_fit_list;
00331     //prepare the actual result for good fits, only these are returned
00332     for (size_t j=0; j < raw_fit_results[i].size(); j++)
00333     {
00334       //get the model pose in the cloud frame by multiplying with table transform
00335       tf::Transform model_trans;
00336       tf::poseMsgToTF(raw_fit_results[i][j].getPose(), model_trans);
00337       model_trans = table_trans * model_trans;
00338       geometry_msgs::Pose model_pose;
00339       tf::poseTFToMsg(model_trans, model_pose);
00340       //create the model fit result
00341       household_objects_database_msgs::DatabaseModelPose pose_msg;
00342       pose_msg.model_id = raw_fit_results[i][j].getModelId();
00343       pose_msg.pose.header = table.pose.header;
00344       pose_msg.pose.pose = model_pose;
00345       pose_msg.confidence = raw_fit_results[i][j].getScore();
00346       //and push it in the list for this cluster
00347       model_potential_fit_list.model_list.push_back(pose_msg);
00348       ROS_DEBUG("  model %d with confidence %f", pose_msg.model_id, pose_msg.confidence);
00349     }
00350     response.models.push_back(model_potential_fit_list);
00351   }
00352   ROS_INFO("Results ready");
00353 
00354 }
00355 
00356 void TabletopObjectRecognizer::publishFitMarkers(
00357                  const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models,
00358                  const Table &table)
00359 {
00360   for (size_t i=0; i<potential_models.size(); i++)
00361   {
00362     const std::vector<household_objects_database_msgs::DatabaseModelPose> models = potential_models[i].model_list;
00363     for (size_t j=0; j<models.size(); j++)
00364     {
00365       if (models[j].confidence > min_marker_quality_) break;
00366       household_objects_database_msgs::GetModelMesh get_mesh;
00367       get_mesh.request.model_id = models[j].model_id;
00368       if ( !get_model_mesh_srv_.call(get_mesh) ||
00369            get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS )
00370       {
00371         ROS_ERROR("tabletop_object_detector: failed to call database get mesh service for marker display");
00372       }
00373       else
00374       {
00375         double rank = ((double)j) / std::max( (int)(models.size())-1, 1 );
00376         visualization_msgs::Marker fitMarker =  MarkerGenerator::getFitMarker(get_mesh.response.mesh, rank);
00377         fitMarker.header = table.pose.header;
00378         fitMarker.pose = models[j].pose.pose;
00379         fitMarker.ns = "tabletop_node_model_" + boost::lexical_cast<std::string>(j);
00380         fitMarker.id = current_marker_id_++;
00381         marker_pub_.publish(fitMarker);
00382       }  
00383     }
00384   }
00385 }
00386 
00387 void TabletopObjectRecognizer::clearOldMarkers(std::string frame_id)
00388 {
00389   for (int id=current_marker_id_; id < num_markers_published_; id++)
00390   {
00391     visualization_msgs::Marker delete_marker;
00392     delete_marker.header.stamp = ros::Time::now();
00393     delete_marker.header.frame_id = frame_id;
00394     delete_marker.id = id;
00395     delete_marker.action = visualization_msgs::Marker::DELETE;
00396     //a hack, but we don't know what namespace the marker was in
00397     for (size_t j=0; j<10; j++)
00398     {
00399       delete_marker.ns = "tabletop_node_model_" + boost::lexical_cast<std::string>(j);
00400       marker_pub_.publish(delete_marker);
00401     }
00402   }
00403   num_markers_published_ = current_marker_id_;
00404   current_marker_id_ = 0;
00405 }
00406 
00407 double TabletopObjectRecognizer::fitDistance(const ModelFitInfo &m1, const ModelFitInfo &m2)
00408 {
00409   double dx = m1.getPose().position.x - m2.getPose().position.x;
00410   double dy = m1.getPose().position.y - m2.getPose().position.y;
00411   double d = dx*dx + dy*dy;
00412   return sqrt(d);
00413 }
00414 
00415 template <class PointCloudType>
00416 double TabletopObjectRecognizer::fitClusterDistance(const ModelFitInfo &m, const PointCloudType &cluster)
00417 {
00418   double dist = 100.0 * 100.0;
00419   double mx = m.getPose().position.x;
00420   double my = m.getPose().position.x;
00421   for (size_t i=0; i<cluster.points.size(); i++)
00422   {
00423     double dx = cluster.points[i].x - mx;
00424     double dy = cluster.points[i].y - my;
00425     double d = dx*dx + dy*dy;
00426     dist = std::min(d, dist);
00427   }
00428   return sqrt(dist);
00429 }
00430 
00431 } //namespace tabletop_object_detector
00432 
00433 int main(int argc, char **argv) 
00434 {
00435   ros::init(argc, argv, "tabletop_object_recognition");
00436   ros::NodeHandle nh;
00437 
00438   tabletop_object_detector::TabletopObjectRecognizer node(nh);
00439   ros::spin();
00440   return 0;
00441 }


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:48:47