exhaustive_fit_detector.h
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 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 #ifndef _EXHAUSTIVE_FIT_DETECTOR_
00038 #define _EXHAUSTIVE_FIT_DETECTOR_
00039 
00040 #include <vector>
00041 #include <set>
00042 #include <boost/filesystem.hpp>
00043 #include <boost/shared_ptr.hpp>
00044 
00045 #include <sensor_msgs/PointCloud.h>
00046 
00047 #include <household_objects_database_msgs/GetModelList.h>
00048 #include <household_objects_database_msgs/GetModelMesh.h>
00049 
00050 #include "tabletop_object_detector/model_fitter.h"
00051 
00052 namespace tabletop_object_detector {
00053 
00055 
00063 template <class Fitter>
00064 class ExhaustiveFitDetector
00065 {
00066  private:
00068   ros::NodeHandle priv_nh_;
00070   ros::NodeHandle nh_;  
00072   std::vector<Fitter*> templates;
00073 
00075   std::set<int> model_exclusion_set_;
00076   bool negate_exclusions_;
00077 
00078  public:
00080  ExhaustiveFitDetector() : priv_nh_("~"), nh_(""), negate_exclusions_(false) {}
00082   ~ExhaustiveFitDetector();
00083 
00084   void addModelToExclusionList(int model_id)
00085   {
00086     model_exclusion_set_.insert(model_id);
00087   }
00088 
00089   void clearExclusionList()
00090   {
00091     model_exclusion_set_.clear();
00092   }
00093 
00094   void setNegateExclusions(bool value)
00095   {
00096     negate_exclusions_ = value;
00097   }
00098 
00100   void loadDatabaseModels(std::string model_set);
00101 
00103 
00108   template <class PointCloudType>
00109   std::vector<ModelFitInfo> fitBestModels(const PointCloudType& cloud, int numResults)
00110   {
00111     std::vector<ModelFitInfo> fit_results;
00112     if (numResults <= 0) return fit_results;
00113     
00114     for (size_t i=0; i<templates.size(); ++i) 
00115     {
00116       ModelFitInfo current = templates[i]->template fitPointCloud<PointCloudType>(cloud);
00117       // If the model that was matched is not in the exclusion list
00118       bool found = (model_exclusion_set_.find(current.getModelId()) != model_exclusion_set_.end());
00119       if (negate_exclusions_ == found)
00120       {
00121         if ((int)fit_results.size() < numResults)
00122         {
00123           fit_results.push_back(current);
00124           std::sort(fit_results.begin(), fit_results.end(), ModelFitInfo::compareScores);
00125         }
00126         else
00127         {
00128           if (fit_results.back().getScore() > current.getScore())
00129           {
00130             fit_results.back() = current;
00131             std::sort(fit_results.begin(), fit_results.end(), ModelFitInfo::compareScores);
00132           }
00133         }
00134       }
00135     } 
00136     return fit_results;
00137   }
00138 };
00139 
00140 template <class Fitter>
00141 ExhaustiveFitDetector<Fitter>::~ExhaustiveFitDetector()
00142 {
00143   for (size_t i=0;i<templates.size();++i) {
00144     delete templates[i];
00145   }
00146 }
00147 
00159 template <class Fitter>
00160 void ExhaustiveFitDetector<Fitter>::loadDatabaseModels(std::string model_set)
00161 {
00162   std::string get_model_list_srv_name;
00163   priv_nh_.param<std::string>("get_model_list_srv", get_model_list_srv_name, "get_model_list_srv");
00164   while ( !ros::service::waitForService(get_model_list_srv_name, ros::Duration(2.0)) && nh_.ok() ) 
00165   {
00166     ROS_INFO("Waiting for %s service to come up", get_model_list_srv_name.c_str());
00167   }
00168   if (!nh_.ok()) exit(0);
00169   ros::ServiceClient get_model_list_srv = nh_.serviceClient<household_objects_database_msgs::GetModelList>
00170     (get_model_list_srv_name, true);
00171 
00172 
00173   std::string get_model_mesh_srv_name;
00174   priv_nh_.param<std::string>("get_model_mesh_srv", get_model_mesh_srv_name, "get_model_mesh_srv");
00175   while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && nh_.ok() ) 
00176   {
00177     ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
00178   }
00179   if (!nh_.ok()) exit(0);
00180   ros::ServiceClient get_model_mesh_srv = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
00181     (get_model_mesh_srv_name, true);
00182 
00183 
00184   household_objects_database_msgs::GetModelList model_list;
00185   model_list.request.model_set = model_set;
00186   if (!get_model_list_srv.call(model_list) || 
00187       model_list.response.return_code.code != model_list.response.return_code.SUCCESS )
00188   {
00189     ROS_ERROR("Could not retrieve list of models from database");
00190     return;
00191   }
00192   if (model_list.response.model_ids.empty()) 
00193   {
00194     ROS_ERROR("Empty model list retrieved from database");
00195     return;
00196   }
00197 
00198   ROS_INFO("Object detector: loading object models");
00199   for(size_t i=0; i<model_list.response.model_ids.size(); i++) 
00200   {
00201     int model_id = model_list.response.model_ids[i];
00202 
00203     household_objects_database_msgs::GetModelMesh get_mesh;
00204     get_mesh.request.model_id = model_id;
00205     if ( !get_model_mesh_srv.call(get_mesh) || 
00206          get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS )
00207     {
00208       ROS_ERROR("Failed to retrieve mesh for model %d", model_id);
00209       continue;
00210     }
00211     if (get_mesh.response.mesh.vertices.empty() || get_mesh.response.mesh.triangles.empty() )
00212     {
00213       ROS_ERROR("Empty mesh for model %d", model_id);
00214       continue;
00215     }
00216 
00217     Fitter* fitter = new Fitter();
00218     fitter->initializeFromMesh(get_mesh.response.mesh);
00219     templates.push_back(fitter);  
00220     //set the model ID in the template so that we can use it later
00221     templates.back()->setModelId( model_id );
00222     ROS_INFO("  Loaded database model with id %d", model_id);
00223   }
00224   ROS_INFO("Object detector: loading complete");
00225 }
00226 
00227 } //namespace
00228 
00229 #endif


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