generate_hypothesis_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 HypothesisGeneratorNode
00012   {
00013   public:
00014     HypothesisGeneratorNode(std::string database)
00015     {
00016       std::string database_dir = database;
00017       std::string debug_folder = "debug_classification/";
00018 
00019       typename pcl17::Feature<pcl17::PointNormal, FeatureType>::Ptr feature_estimator(new FeatureEstimatorType);
00020       oc.setFeatureEstimator(feature_estimator);
00021 
00022       oc.setDatabaseDir(database_dir);
00023       oc.loadFromFile();
00024 
00025       //oc.setDebugFolder(debug_folder);
00026       oc.setDebug(false);
00027 
00028       ros::NodeHandle nh;
00029       pub = nh.advertise<furniture_classification::Hypothesis> ("/furniture_hypothesis", 1);
00030       sub = nh.subscribe<pcl17::PointCloud<pcl17::PointXYZ> > ("/cloud_pcd", 1, &HypothesisGeneratorNode::cloud_cb,
00031                                                                this);
00032 
00033       std::map<std::string, vector<pcl17::PointCloud<pcl17::PointNormal>::Ptr> >::iterator iter;
00034       for (iter = oc.class_name_to_full_models_map_.begin(); iter != oc.class_name_to_full_models_map_.end(); iter++)
00035       {
00036         votes_publisher[iter->first] = nh.advertise<pcl17::PointCloud<pcl17::PointXYZ> > ("/" + iter->first + "_votes",
00037                                                                                           1);
00038       }
00039 
00040     }
00041 
00042     void cloud_cb(const typename pcl17::PointCloud<pcl17::PointXYZ>::ConstPtr& msg)
00043     {
00044       oc.setScene(msg);
00045       //std::cout << "Recieved pointcloud" << std::endl;
00046       std::map<std::string, pcl17::PointCloud<pcl17::PointXYZ>::Ptr> votes_map;
00047       pub.publish(oc.generate_hypothesis(votes_map));
00048 
00049       std::map<std::string, pcl17::PointCloud<pcl17::PointXYZ>::Ptr>::iterator iter;
00050       for (iter = votes_map.begin(); iter != votes_map.end(); iter++)
00051       {
00052           iter->second->header.frame_id = msg->header.frame_id;
00053         votes_publisher[iter->first].publish(iter->second);
00054       }
00055 
00056     }
00057 
00058     pcl17::PHVObjectClassifier<pcl17::PointXYZ, pcl17::PointNormal, FeatureType> oc;
00059     ros::Publisher pub;
00060     ros::Subscriber sub;
00061     std::map<std::string, ros::Publisher> votes_publisher;
00062 
00063   };
00064 
00065 int main(int argc, char **argv)
00066 {
00067 
00068   if (argc < 5)
00069   {
00070     PCL17_INFO("Usage %s -database /path/to/database -features sgf | esf | vfh \n", argv[0]);
00071     return -1;
00072   }
00073 
00074   ros::init(argc, argv, "hypothesis_generator");
00075 
00076   std::string database;
00077   std::string features = "sgf";
00078 
00079   pcl17::console::parse_argument(argc, argv, "-database", database);
00080   pcl17::console::parse_argument(argc, argv, "-features", features);
00081 
00082   if (features == "sgf")
00083   {
00084     HypothesisGeneratorNode<pcl17::Histogram<pcl17::SGFALL_SIZE>, pcl17::SGFALLEstimation<pcl17::PointNormal,
00085         pcl17::Histogram<pcl17::SGFALL_SIZE> > > v(database);
00086     ros::spin();
00087   }
00088   else if (features == "esf")
00089   {
00090     HypothesisGeneratorNode<pcl17::ESFSignature640, pcl17::ESFEstimation<pcl17::PointNormal, pcl17::ESFSignature640> >
00091                                                                                                                        v(
00092                                                                                                                          database);
00093     ros::spin();
00094   }
00095   else if (features == "vfh")
00096   {
00097     HypothesisGeneratorNode<pcl17::VFHSignature308, pcl17::VFHEstimation<pcl17::PointNormal, pcl17::PointNormal,
00098         pcl17::VFHSignature308> > v(database);
00099     ros::spin();
00100   }
00101   else
00102   {
00103     std::cerr << "Unknown feature type " << features << " specified" << std::endl;
00104   }
00105 
00106   return 0;
00107 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Thu May 23 2013 18:32:28