int main(int argc, char** argv) { if (argc != 3) { printUsage(argv); return -1; } std::string filename_in = argv[1]; std::string filename_out = argv[2]; // read in printf("Reading cloud\n"); PointCloudT::Ptr cloud; cloud.reset(new rgbdtools::PointCloudT()); pcl::PCDReader reader; reader.read(filename_in, *cloud); alignGlobalCloud(cloud); return 0; }
int main (int argc, char** argv) { PointCloudT::Ptr cloud (new PointCloudT); PointCloudT::Ptr new_cloud (new PointCloudT); bool new_cloud_available_flag = false; //pcl::Grabber* grab = new pcl::OpenNIGrabber (); PointCloudT::Ptr ddd; boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag); grab->registerCallback (f); viewer->registerKeyboardCallback(keyboardEventOccurred); grab->start (); bool first_time = true; FILE* objects; objects = fopen ("objects.txt","a"); while(!new_cloud_available_flag) boost::this_thread::sleep(boost::posix_time::milliseconds(1)); new_cloud_available_flag=false; //////////////////// // invert correction //////////////////// Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity(); transMat (1,1) = -1; //////////////////// // pass filter //////////////////// PointCloudT::Ptr passed_cloud; pcl::PassThrough<PointT> pass; passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // voxel grid //////////////////// PointCloudT::Ptr voxelized_cloud; voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); pcl::VoxelGrid<PointT> vg; vg.setLeafSize (0.001, 0.001, 0.001); //////////////////// // sac segmentation //////////////////// PointCloudT::Ptr cloud_f; PointCloudT::Ptr cloud_plane; PointCloudT::Ptr cloud_filtered; cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT); cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT); cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT); pcl::SACSegmentation<PointT> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); //////////////////// // euclidean clustering //////////////////// std::vector<pcl::PointIndices> cluster_indices; std::vector<PointCloudT::Ptr> extracted_clusters; pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>); pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance (0.04); ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (eucl_tree); PointCloudT::Ptr cloud_cluster; //////////////////// // vfh estimate //////////////////// pcl::NormalEstimation<PointT, pcl::Normal> ne; pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ()); pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh; pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ()); std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs; ne.setSearchMethod (vfh_tree1); ne.setRadiusSearch (0.05); vfh.setSearchMethod (vfh_tree2); vfh.setRadiusSearch (0.05); pcl::PointCloud<pcl::Normal>::Ptr normals; pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs; //////////////////// // nearest neighbour //////////////////// int k = 6; std::string kdtree_idx_file_name = "kdtree.idx"; std::string training_data_h5_file_name = "training_data.h5"; std::string training_data_list_file_name = "training_data.list"; // std::vector<vfh_model> models; // flann::Matrix<float> data; // loadFileList (models, training_data_list_file_name); // flann::load_from_file (data, // training_data_h5_file_name, // "training_data"); // // flann::Index<flann::ChiSquareDistance<float> > index (data, // flann::SavedIndexParams // ("kdtree.idx")); //////////////////// // process nearest neighbour //////////////////// std::vector<hypothesis> final_hypothesis; final_hypothesis.clear(); double last = pcl::getTime(); while (! viewer->wasStopped()) { if (new_cloud_available_flag) { new_cloud_available_flag = false; double now = pcl::getTime(); //////////////////// // pass filter //////////////////// //passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // voxel grid //////////////////// //voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // sac segmentation //////////////////// //cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT); //cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT); //cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT); //////////////////// // euclidean clustering //////////////////// extracted_clusters.clear(); cluster_indices.clear(); //////////////////// // vfh estimate //////////////////// computed_vfhs.clear(); //////////////////// // nearest neighbour //////////////////// cloud_mutex.lock(); //displayCloud(viewer,cloud); boost::thread displayCloud_(displayCloud,viewer,cloud); if(now-last > 13 || first_time) { first_time = false; last=now; //////////////////// // invert correction //////////////////// pcl::transformPointCloud(*cloud,*new_cloud,transMat); //////////////////// // pass filter //////////////////// pass.setInputCloud (new_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.5, 0.5); //pass.setFilterLimitsNegative (true); pass.filter (*passed_cloud); //////////////////// // voxel grid //////////////////// vg.setInputCloud (passed_cloud); vg.filter (*voxelized_cloud); //////////////////// // sac segmentation //////////////////// *cloud_filtered = *voxelized_cloud; int i=0, nr_points = (int) voxelized_cloud->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Couldnt estimate a planar model for the dataset.\n"; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<PointT> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } //////////////////// // euclidean clustering //////////////////// // Creating the KdTree object for the search method of the extraction eucl_tree->setInputCloud (cloud_filtered); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { //PointCloudT::Ptr cloud_cluster (new PointCloudT); cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_filtered->points [*pit]); cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; extracted_clusters.push_back(cloud_cluster); } cloud_cluster.reset(); //////////////////// // vfh estimate //////////////////// for (int z=0; z<extracted_clusters.size(); ++z) { vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>); normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>); ne.setInputCloud (extracted_clusters.at(z)); ne.compute (*normals); vfh.setInputCloud (extracted_clusters.at(z)); vfh.setInputNormals (normals); vfh.compute (*vfhs); computed_vfhs.push_back(vfhs); } vfhs.reset(); normals.reset(); //////////////////// // nearest neighbour //////////////////// std::vector<vfh_model> models; flann::Matrix<int> k_indices; flann::Matrix<float> k_distances; flann::Matrix<float> data; loadFileList (models, training_data_list_file_name); flann::load_from_file (data, training_data_h5_file_name, "training_data"); flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx")); for(int z=0; z<computed_vfhs.size(); ++z) { vfh_model histogram; histogram.second.resize(308); for (size_t i = 0; i < 308; ++i) { histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i]; } index.buildIndex (); nearestKSearch (index, histogram, k, k_indices, k_distances); hypothesis x; x.distance = k_distances[0][0]; size_t index = models.at(k_indices[0][0]).first.find("_v",5); x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5); ddd = boost::shared_ptr<PointCloudT>(new PointCloudT); pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat); x.cluster = ddd; ddd.reset(); std::string filename =""; filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1); filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; x.cluster_name = filename.c_str(); final_hypothesis.push_back(x); x.cluster.reset(); //delete x; // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1); // filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; // const char* filen = filename.c_str(); // fprintf(objects,"%s",filen); // fprintf(objects,"::"); // fprintf(objects,models.at (k_indices[0][0]).first.c_str()); // fprintf(objects,"::"); // fprintf(objects,"%f",k_distances[0][0]); // fprintf(objects,"\n"); } delete[] k_indices.ptr (); delete[] k_distances.ptr (); delete[] data.ptr (); std::cout << final_hypothesis.size() << std::endl; viewer->removeAllShapes(); for(int z=0; z<final_hypothesis.size();++z) { if(final_hypothesis.at(z).distance < 100) { fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str()); fprintf(objects,"::"); fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str()); fprintf(objects,"::"); fprintf(objects,"%f",final_hypothesis.at(z).distance); fprintf(objects,"\n"); std::stringstream ddd; ddd << final_hypothesis.at(z).object_name; ddd << "\n" << "("; ddd << final_hypothesis.at(z).distance; ddd << ")"; viewer->addText3D(ddd.str().c_str(), final_hypothesis.at(z).cluster->points[0], 0.02,1,1,1, boost::lexical_cast<std::string>(z)); drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z); } } //boost::thread allBoxes_(allBoxes,viewer,final_hypothesis); //allBoxes_.join(); viewer->spinOnce(); final_hypothesis.clear(); j++; } // for(int z=0; z<extracted_clusters.size(); ++z) // { // //viewer->addPointCloud<PointT>(extracted_clusters.at(z), // // boost::lexical_cast<std::string>(z)); // // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j); // filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; // pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false); // } // for(int z=0; z<computed_vfhs.size(); ++z) // { // //viewer->addPointCloud<PointT>(extracted_clusters.at(z), // // boost::lexical_cast<std::string>(z)); // // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j); // filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd"; // pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z)); // } //viewer->removeAllShapes(); // viewer->removeAllPointClouds(); // viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0); // pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); // viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud"); // viewer->spinOnce(); //std::cout << final_hypothesis.at(0).cluster->points[0]; //boost::this_thread::sleep(boost::posix_time::milliseconds(10)); displayCloud_.join(); cloud_mutex.unlock(); } } grab->stop(); return 0; }