fit_models_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <pcl17_ros/point_cloud.h>
00003 #include <pcl17/console/print.h>
00004 #include <pcl17/classification/PHVObjectClassifier.h>
00005 #include <pcl17/features/sgfall.h>
00006 #include <pcl17/console/parse.h>
00007 #include <pcl17/features/vfh.h>
00008 #include <pcl17/features/esf.h>
00009 
00010 template<class FeatureType, class FeatureEstimatorType>
00011   class FitModelsNode
00012   {
00013   public:
00014     FitModelsNode(std::string database, int n)
00015     {
00016       std::string database_dir = database;
00017       std::string debug_folder = "debug_classification/";
00018       std::string n_str = boost::lexical_cast<std::string>(n);
00019 
00020       typename pcl17::Feature<pcl17::PointNormal, FeatureType>::Ptr feature_estimator(new FeatureEstimatorType);
00021       oc.setFeatureEstimator(feature_estimator);
00022 
00023       oc.setDatabaseDir(database_dir);
00024       oc.loadFromFile();
00025 
00026       //oc.setDebugFolder(debug_folder);
00027       oc.setDebug(false);
00028 
00029       ros::NodeHandle nh;
00030       pub = nh.advertise<furniture_classification::FittedModels> ("/fitted_models" + n_str, 1);
00031       sub = nh.subscribe<pcl17::PointCloud<pcl17::PointXYZ> > ("/cloud_pcd", 1, &FitModelsNode::cloud_cb,
00032                                                                this);
00033 
00034       sub_hp = nh.subscribe<furniture_classification::Hypothesis> ("/furniture_hypothesis" + n_str , 1, &FitModelsNode::cloud_hp,
00035                                                                this);
00036 
00037     }
00038 
00039     void cloud_cb(const typename pcl17::PointCloud<pcl17::PointXYZ>::ConstPtr& msg)
00040     {
00041       oc.setScene(msg);
00042     }
00043 
00044     void cloud_hp(const typename furniture_classification::Hypothesis::ConstPtr & msg)
00045     {
00046       if(oc.getScene() == NULL) return;
00047       std::cerr << "Started fitting" << std::endl;
00048       furniture_classification::FittedModelsPtr res = oc.fit_objects(msg);
00049       res->frame_id = oc.getScene()->header.frame_id;
00050       //res->header.frame_id = oc.getScene()->header.frame_id;
00051       std::cerr << "Finished fitting with "<< res->models.size() << " models " << std::endl;
00052       pub.publish(res);
00053 
00054     }
00055 
00056     pcl17::PHVObjectClassifier<pcl17::PointXYZ, pcl17::PointNormal, FeatureType> oc;
00057     ros::Publisher pub;
00058     ros::Subscriber sub;
00059     ros::Subscriber sub_hp;
00060 
00061   };
00062 
00063 int main(int argc, char **argv)
00064 {
00065 
00066   if (argc < 5)
00067   {
00068     PCL17_INFO("Usage %s -database /path/to/database -features sgf | esf | vfh \n", argv[0]);
00069     return -1;
00070   }
00071 
00072 
00073 
00074   std::string database;
00075   std::string features = "sgf";
00076   int n=0;
00077 
00078   pcl17::console::parse_argument(argc, argv, "-database", database);
00079   pcl17::console::parse_argument(argc, argv, "-features", features);
00080   pcl17::console::parse_argument(argc, argv, "-n", n);
00081 
00082   ros::init(argc, argv, "model_fitter" + boost::lexical_cast<std::string>(n));
00083 
00084 
00085   if (features == "sgf")
00086   {
00087     FitModelsNode<pcl17::Histogram<pcl17::SGFALL_SIZE>, pcl17::SGFALLEstimation<pcl17::PointNormal, pcl17::Histogram<
00088         pcl17::SGFALL_SIZE> > > v(database, n);
00089     ros::spin();
00090   }
00091   else if (features == "esf")
00092   {
00093     FitModelsNode<pcl17::ESFSignature640, pcl17::ESFEstimation<pcl17::PointNormal, pcl17::ESFSignature640> >
00094                                                                                                              v(database,n);
00095     ros::spin();
00096   }
00097   else if (features == "vfh")
00098   {
00099     FitModelsNode<pcl17::VFHSignature308, pcl17::VFHEstimation<pcl17::PointNormal, pcl17::PointNormal,
00100         pcl17::VFHSignature308> > v(database,n);
00101     ros::spin();
00102   }
00103   else
00104   {
00105     std::cerr << "Unknown feature type " << features << " specified" << std::endl;
00106   }
00107 
00108   return 0;
00109 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Sun Oct 6 2013 12:12:31