Go to the documentation of this file.00001 #include <iostream>
00002 #include <pcl/io/pcd_io.h>
00003 #include <pcl/features/normal_3d.h>
00004 #include <pcl/features/feature.h>
00005 #include <pcl/visualization/cloud_viewer.h>
00006 #include <pcl/features/fpfh.h>
00007 #include <pcl/features/impl/fpfh.hpp>
00008 #include <pcl/recognition/implicit_shape_model.h>
00009 #include <pcl/recognition/impl/implicit_shape_model.hpp>
00010
00011 int
00012 main (int argc, char** argv)
00013 {
00014 if (argc == 0 || argc % 2 == 0)
00015 return (-1);
00016
00017 unsigned int number_of_training_clouds = (argc - 3) / 2;
00018
00019 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
00020 normal_estimator.setRadiusSearch (25.0);
00021
00022 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> training_clouds;
00023 std::vector<pcl::PointCloud<pcl::Normal>::Ptr> training_normals;
00024 std::vector<unsigned int> training_classes;
00025
00026 for (unsigned int i_cloud = 0; i_cloud < number_of_training_clouds - 1; i_cloud++)
00027 {
00028 pcl::PointCloud<pcl::PointXYZ>::Ptr tr_cloud(new pcl::PointCloud<pcl::PointXYZ> ());
00029 if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[i_cloud * 2 + 1], *tr_cloud) == -1 )
00030 return (-1);
00031
00032 pcl::PointCloud<pcl::Normal>::Ptr tr_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();
00033 normal_estimator.setInputCloud (tr_cloud);
00034 normal_estimator.compute (*tr_normals);
00035
00036 unsigned int tr_class = static_cast<unsigned int> (strtol (argv[i_cloud * 2 + 2], 0, 10));
00037
00038 training_clouds.push_back (tr_cloud);
00039 training_normals.push_back (tr_normals);
00040 training_classes.push_back (tr_class);
00041 }
00042
00043 pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >::Ptr fpfh
00044 (new pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<153> >);
00045 fpfh->setRadiusSearch (30.0);
00046 pcl::Feature< pcl::PointXYZ, pcl::Histogram<153> >::Ptr feature_estimator(fpfh);
00047
00048 pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal> ism;
00049 ism.setFeatureEstimator(feature_estimator);
00050 ism.setTrainingClouds (training_clouds);
00051 ism.setTrainingNormals (training_normals);
00052 ism.setTrainingClasses (training_classes);
00053 ism.setSamplingSize (2.0f);
00054
00055 pcl::ism::ImplicitShapeModelEstimation<153, pcl::PointXYZ, pcl::Normal>::ISMModelPtr model = boost::shared_ptr<pcl::features::ISMModel>
00056 (new pcl::features::ISMModel);
00057 ism.trainISM (model);
00058
00059 std::string file ("trained_ism_model.txt");
00060 model->saveModelToFile (file);
00061
00062 model->loadModelFromfile (file);
00063
00064 unsigned int testing_class = static_cast<unsigned int> (strtol (argv[argc - 1], 0, 10));
00065 pcl::PointCloud<pcl::PointXYZ>::Ptr testing_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00066 if ( pcl::io::loadPCDFile <pcl::PointXYZ> (argv[argc - 2], *testing_cloud) == -1 )
00067 return (-1);
00068
00069 pcl::PointCloud<pcl::Normal>::Ptr testing_normals = (new pcl::PointCloud<pcl::Normal>)->makeShared ();
00070 normal_estimator.setInputCloud (testing_cloud);
00071 normal_estimator.compute (*testing_normals);
00072
00073 boost::shared_ptr<pcl::features::ISMVoteList<pcl::PointXYZ> > vote_list = ism.findObjects (
00074 model,
00075 testing_cloud,
00076 testing_normals,
00077 testing_class);
00078
00079 double radius = model->sigmas_[testing_class] * 10.0;
00080 double sigma = model->sigmas_[testing_class];
00081 std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > strongest_peaks;
00082 vote_list->findStrongestPeaks (strongest_peaks, testing_class, radius, sigma);
00083
00084 pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
00085 colored_cloud->height = 0;
00086 colored_cloud->width = 1;
00087
00088 pcl::PointXYZRGB point;
00089 point.r = 255;
00090 point.g = 255;
00091 point.b = 255;
00092
00093 for (size_t i_point = 0; i_point < testing_cloud->points.size (); i_point++)
00094 {
00095 point.x = testing_cloud->points[i_point].x;
00096 point.y = testing_cloud->points[i_point].y;
00097 point.z = testing_cloud->points[i_point].z;
00098 colored_cloud->points.push_back (point);
00099 }
00100 colored_cloud->height += testing_cloud->points.size ();
00101
00102 point.r = 255;
00103 point.g = 0;
00104 point.b = 0;
00105 for (size_t i_vote = 0; i_vote < strongest_peaks.size (); i_vote++)
00106 {
00107 point.x = strongest_peaks[i_vote].x;
00108 point.y = strongest_peaks[i_vote].y;
00109 point.z = strongest_peaks[i_vote].z;
00110 colored_cloud->points.push_back (point);
00111 }
00112 colored_cloud->height += strongest_peaks.size ();
00113
00114 pcl::visualization::CloudViewer viewer ("Result viewer");
00115 viewer.showCloud (colored_cloud);
00116 while (!viewer.wasStopped ())
00117 {
00118 }
00119
00120 return (0);
00121 }