classify_live.cpp
Go to the documentation of this file.
00001 #include <pcl17/console/print.h>
00002 #include <pcl17/io/openni_grabber.h>
00003 #include <pcl17/visualization/cloud_viewer.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 SimpleOpenNIViewer
00012   {
00013   public:
00014     SimpleOpenNIViewer(std::string database) :
00015       viewer("PCL OpenNI Viewer"), classify(false), classificaton_running(false), interface(new pcl17::OpenNIGrabber())
00016     {
00017       std::string database_dir = database;
00018       std::string debug_folder = "debug_classification/";
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(true);
00028 
00029     }
00030 
00031     void cloud_cb_(const pcl17::PointCloud<pcl17::PointXYZRGBA>::ConstPtr &cloud)
00032     {
00033       if (viewer.wasStopped())
00034         return;
00035 
00036       if (!classify && !classificaton_running)
00037       {
00038         viewer.showCloud(convert(cloud, 30));
00039       }
00040       else if (classify && !classificaton_running)
00041       {
00042         classificaton_running = true;
00043         interface->stop();
00044         //viewer.showCloud(cloud);
00045 
00046         pcl17::PointCloud<pcl17::PointXYZRGBA>::Ptr cloud_transformed = convert(cloud, 30);
00047 
00048         pcl17::PointCloud<pcl17::PointXYZ>::Ptr scene(new pcl17::PointCloud<pcl17::PointXYZ>);
00049 
00050         pcl17::copyPointCloud(*cloud_transformed, *scene);
00051 
00052         scene->sensor_origin_ = cloud_transformed->sensor_origin_;
00053         scene->sensor_orientation_ = cloud_transformed->sensor_orientation_;
00054 
00055         std::cerr << "Added scene" << std::endl;
00056         oc.setScene(scene);
00057         std::cerr << "Classifying" << std::endl;
00058         oc.classify();
00059 
00060         map<string, vector<pcl17::PointCloud<pcl17::PointNormal>::Ptr> > objects = oc.getFoundObjects();
00061 
00062         typedef typename map<string, vector<pcl17::PointCloud<pcl17::PointNormal>::Ptr> >::value_type vt;
00063 
00064         BOOST_FOREACH(vt &v, objects)
00065 {        for(size_t i=0; i<v.second.size(); i++)
00066         {
00067           pcl17::PointCloud<pcl17::PointXYZRGBA>::Ptr model(new pcl17::PointCloud<pcl17::PointXYZRGBA>);
00068           pcl17::copyPointCloud(*v.second[i], *model);
00069 
00070           for(size_t i=0; i<model->points.size(); i++)
00071           {
00072             model->points[i].r = 255;
00073             model->points[i].g = 0;
00074             model->points[i].b = 0;
00075           }
00076 
00077           *cloud_transformed += *model;
00078 
00079         }
00080       }
00081 
00082       viewer.showCloud(cloud_transformed);
00083 
00084     }
00085   }
00086 
00087   void keyboard_cb(const pcl17::visualization::KeyboardEvent &event, void * tmp)
00088   {
00089     if (event.getKeySym() == "c" && event.keyDown())
00090     {
00091       classify = true;
00092     }
00093   }
00094 
00095   void run()
00096   {
00097 
00098     boost::function<void(const pcl17::PointCloud<pcl17::PointXYZRGBA>::ConstPtr&)> f =
00099     boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);
00100 
00101     //boost::function<void (const pcl17::visualization::KeyboardEvent&)> k =
00102     //        boost::bind(&SimpleOpenNIViewer::keyboard_cb, this, _1);
00103 
00104     interface->registerCallback(f);
00105     viewer.registerKeyboardCallback(&SimpleOpenNIViewer::keyboard_cb, *this, NULL);
00106 
00107     interface->start();
00108 
00109     while (!viewer.wasStopped())
00110     {
00111       boost::this_thread::sleep(boost::posix_time::seconds(1));
00112     }
00113 
00114     interface->stop();
00115   }
00116 
00117   pcl17::PointCloud<pcl17::PointXYZRGBA>::Ptr convert(pcl17::PointCloud<pcl17::PointXYZRGBA>::ConstPtr scene, int tilt)
00118   {
00119 
00120     pcl17::PointCloud<pcl17::PointXYZRGBA>::Ptr cloud_transformed(new pcl17::PointCloud<pcl17::PointXYZRGBA>), cloud_aligned(new pcl17::PointCloud<pcl17::PointXYZRGBA>);
00121 
00122     Eigen::Affine3f view_transform;
00123     view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
00124 
00125     Eigen::AngleAxis<float> rot(tilt * M_PI / 180, Eigen::Vector3f(0, 1, 0));
00126 
00127     view_transform.prerotate(rot);
00128 
00129     pcl17::transformPointCloud(*scene, *cloud_transformed, view_transform);
00130 
00131     pcl17::ModelCoefficients::Ptr coefficients(new pcl17::ModelCoefficients);
00132     pcl17::PointIndices::Ptr inliers(new pcl17::PointIndices);
00133 
00134     pcl17::SACSegmentation<pcl17::PointXYZRGBA> seg;
00135 
00136     seg.setOptimizeCoefficients(true);
00137 
00138     seg.setModelType(pcl17::SACMODEL_PLANE);
00139     seg.setMethodType(pcl17::SAC_RANSAC);
00140     seg.setDistanceThreshold(0.01);
00141 
00142     seg.setInputCloud(cloud_transformed);
00143     seg.segment(*inliers, *coefficients);
00144 
00145     std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
00146     << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
00147 
00148     Eigen::Vector3f z_current(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
00149     Eigen::Vector3f y(0, 1, 0);
00150 
00151     Eigen::Affine3f rotation;
00152     rotation = pcl17::getTransFromUnitVectorsZY(z_current, y);
00153     rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
00154 
00155     pcl17::transformPointCloud(*cloud_transformed, *cloud_aligned, rotation);
00156 
00157     Eigen::Affine3f res = (rotation * view_transform);
00158 
00159     cloud_aligned->sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
00160     cloud_aligned->sensor_orientation_ = res.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
00161 
00162     seg.setInputCloud(cloud_aligned);
00163     seg.segment(*inliers, *coefficients);
00164 
00165     std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " "
00166     << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
00167 
00168     return cloud_aligned;
00169 
00170   }
00171 
00172   pcl17::visualization::CloudViewer viewer;
00173 
00174   bool classify;
00175   bool classificaton_running;
00176   pcl17::PHVObjectClassifier<pcl17::PointXYZ, pcl17::PointNormal, FeatureType> oc;
00177   pcl17::Grabber* interface;
00178 
00179 };
00180 
00181 int main(int argc, char **argv)
00182 {
00183 
00184   if (argc < 5)
00185   {
00186     PCL17_INFO ("Usage %s -database /path/to/database -features sgf | esf | vfh \n", argv[0]);
00187     return -1;
00188   }
00189 
00190   std::string database;
00191   std::string features = "sgf";
00192 
00193   pcl17::console::parse_argument(argc, argv, "-database", database);
00194   pcl17::console::parse_argument(argc, argv, "-features", features);
00195 
00196   if (features == "sgf")
00197   {
00198     SimpleOpenNIViewer<pcl17::Histogram<pcl17::SGFALL_SIZE>, pcl17::SGFALLEstimation<pcl17::PointNormal, pcl17::Histogram<
00199         pcl17::SGFALL_SIZE> > > v(database);
00200     v.run();
00201   }
00202   else if (features == "esf")
00203   {
00204     SimpleOpenNIViewer<pcl17::ESFSignature640, pcl17::ESFEstimation<pcl17::PointNormal, pcl17::ESFSignature640> > v(database);
00205     v.run();
00206   }
00207   else if (features == "vfh")
00208   {
00209     SimpleOpenNIViewer<pcl17::VFHSignature308, pcl17::VFHEstimation<pcl17::PointNormal, pcl17::PointNormal,
00210         pcl17::VFHSignature308> > v(database);
00211     v.run();
00212   }
00213   else
00214   {
00215     std::cerr << "Unknown feature type " << features << " specified" << std::endl;
00216   }
00217 
00218   return 0;
00219 }
 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:27