// 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();
}
