// For debugging void initializeCFGdebug(const std::string & pcd_name) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cfg; cloud_cfg.reset(new pcl::PointCloud<pcl::PointXYZRGB>); reader.read(pcd_name, *cloud_cfg); std::cerr << "PointCloud has: " << cloud_cfg->points.size() << " data points." << std::endl; pcl::VoxelGrid<pcl::PointXYZRGB> pvg; pvg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE); pvg.setInputCloud(cloud_cfg); pvg.filter(*cloud_cfg); std::cout << "PointCloud after prefiltering has: " << cloud_cfg->points.size() << " data points." << std::endl; initializeFromCloud(cloud_cfg); }
int main (int argc, char *argv[]) { if ( argc == 3 ) { if (reader.read (argv[1], *scene) < 0) { std::cout << "Error loading scene cloud." << std::endl; exit (0); } std::string fileName(argv[1]); importObjectsInformation(argv[2]); displayObjects(); std::string folderName = createDir(fileName); extractPCD(_objectList, folderName); } else std::cout<<"Usage: " << argv[0] << " <filename>.pcd <filename>.xml" << std::endl; }
void ExtractionIteration(std::vector<std::string> fileNamesV, fs::path rootFolder) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scene(new pcl::PointCloud<pcl::PointXYZRGBA>()); std::ofstream myfile; std::string folder = createDir(); int i =0; //Extract all the objects from all the pcd in the root directory std::cout<< "No. of files = " << fileNamesV.size() << std::endl; for (std::vector<std::string>::iterator it = fileNamesV.begin(); it != fileNamesV.end(); ++it) { std::string pcdFile = *it + ".pcd"; std::string xmlFile = *it + ".xml"; if (reader.read (pcdFile, *scene) < 0) { std::cout << "Error loading scene cloud from : " << pcdFile << std::endl; exit (0); } if(!fs::exists(xmlFile)) { std::cout<<"Corresponding .xml file not found."<<std::endl; exit(0); } importObjectsInformation(xmlFile.c_str()); //displayObjects(); std::size_t found = it->find_last_of("/\\"); std::string filNam = it->substr(found+1); extractPCD(_objectList, folder, filNam, scene); //Extract object clusters as .pcd files into the created directory _objectList.clear(); } }
void initialize(const std::string & pcd_name) { // Read in the cloud data reader.read(pcd_name, *cloud); std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl; // pcl::io::loadPCDFile(pcd_name, *cloud); // A passthrough filter to remove spurious NaNs pass.setInputCloud(cloud); pass.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; // Downsample the dataset using a leaf size of 1cm vg.setInputCloud(cloud_filtered); vg.filter(*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; // Estimate point normals ne.setInputCloud(cloud_filtered); ne.compute(*cloud_normals); ROS_INFO("%lu normals estimated", cloud_normals->points.size()); }
void initializeFromFilename(const std::string & pcd_name) { reader.read(pcd_name, *cloud); std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl; initialize(true); }
void CorrespondenceIteration(std::vector<std::string> fileNames, fs::path rootFolder) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scene(new pcl::PointCloud<pcl::PointXYZRGBA>()); std::ofstream myfile; std::string resultFile = rootFolder.string() + "/Result.txt"; myfile.open (resultFile.c_str()); for (std::vector<std::string>::iterator it = fileNames.begin(); it != fileNames.end(); ++it) { std::string pcdFile = *it + ".pcd"; std::string xmlFile = *it + ".xml"; if (reader.read (pcdFile, *scene) < 0) { std::cout << "Error loading scene cloud." << std::endl; exit (0); } importObjectsInformation(xmlFile.c_str()); //displayObjects(); createDir(*it); extractPCD(_objectList, *it, scene); _objectList.clear(); fs::path models_path( fs::initial_path<fs::path>()); models_path = fs::system_complete(fs::path(*it)); fs::directory_iterator it_s(rootFolder); fs::directory_iterator endit_s; myfile << "-----------------------------------------------------------------------------------------" << std::endl; myfile << *it << std::endl; myfile << "-----------------------------------------------------------------------------------------" << std::endl; while(it_s != endit_s) //for every scene { if(fs::is_regular_file(*it_s) && it_s->path().extension() == ".pcd") { std::string scene_filename = rootFolder.string() + it_s->path().filename().string(); fs::directory_iterator it_m(models_path); fs::directory_iterator endit_m; while(it_m != endit_m) //for every model { if(fs::is_regular_file(*it_m) && it_m->path().extension() == ".pcd") { std::string model_filename = models_path.string() + "/" + it_m->path().filename().string(); // DO CORRESPONDENCE GROUPING //std::cout << scene_filename << " <<<<<>>>>> " << model_filename << std::endl; //std::cout << it_s->path().filename().string() << " <<<<<>>>>> " << it_m->path().filename().string() << std::endl; //std::cout << "scene_" << j << " model_" << i << " Correspondence: " << correspondenceGrouping(model_filename,scene_filename) <<std::endl; keypointExtraction (model_filename,scene_filename); descriptorComputation (); myfile << it_s->path().filename().string() << " <<<<<--->>>>> " << it_m->path().filename().string()<< " : " << correspondenceGrouping(findingCorrespondence()) <<std::endl; } ++it_m; } } ++it_s; } } myfile.close(); }