// -----------------------------------------------------------------------------------------------------
// generatePCD
// -----------------------------------------------------------------------------------------------------
void PointCloud::generatePCD(const PoseVector &cameraPoses, const char *filenamePCD)
{
char buf_full[256];
pcl::PointCloud<pcl::PointXYZRGB> cloudFull;
pcl::PointCloud<pcl::PointXYZRGB> cloudFrame;
pcl::PointCloud<pcl::PointXYZRGB> cloudFrameTransformed;
int nbPCD=0;
for (int iPose=0; iPose<cameraPoses.size(); iPose++)
{
if (iPose>0 && iPose%Config::_PcdRatioFrame != 0)
continue; // skip frame
cout << "----------------------------------------------------------------------------\n";
cout << "Append point cloud frame=#" << cameraPoses[iPose]._id;
cout << " pose=" << iPose+1 << "/" << cameraPoses.size();
cout << " ratio_pcd=1/" << Config::_PcdRatioFrame;
cout << " cloud=" << (iPose/Config::_PcdRatioFrame)+1 << "/" << ((cameraPoses.size()-1)/Config::_PcdRatioFrame)+1 << "\n";
cout << cameraPoses[iPose]._matrix << std::endl;
getFramePointCloud(cameraPoses[iPose]._id, cloudFrame);
// subsample frame
subsamplePointCloud(cloudFrame, Config::_PcdRatioKeepSubsample);
// apply transformation to the point cloud
pcl::transformPointCloud(
cloudFrame,
cloudFrameTransformed,
Eigen::Affine3f(cameraPoses[iPose]._matrix));
// apend transformed point cloud
cloudFull += cloudFrameTransformed;
cout << " Total Size: " << cloudFull.size() << " points (";
cout << cloudFull.size()*100/Config::_PcdMaxNbPoints << "% of max capacity)." << std::endl;
if (cloudFull.size() > Config::_PcdMaxNbPoints)
{
// max size reached
cout << "Saving global point cloud binary...\n";
sprintf(buf_full, "%s/%s_%02d.pcd", Config::_PathDataProd.c_str(), filenamePCD, nbPCD++);
cout << "File: " << buf_full << "\n";
pcl::io::savePCDFile(buf_full, cloudFull, true);
// bug in PCL - the binary file is not created with the good permissions!
char bufsys[256];
sprintf(bufsys, "chmod a+rw %s", buf_full);
system(bufsys);
cloudFull.points.clear();
}
}
if (cloudFull.size()>0)
{
// subsample final point cloud
//subsamplePointCloud(cloudFull, Config::_PcdRatioKeepSubsample);
cout << "Saving global point cloud binary...\n";
if (nbPCD>0)
sprintf(buf_full, "%s/%s_%02d.pcd", Config::_PathDataProd.c_str(), filenamePCD, nbPCD);
else
sprintf(buf_full, "%s/%s.pcd", Config::_PathDataProd.c_str(), filenamePCD);
cout << "File: " << buf_full << "\n";
pcl::io::savePCDFile(buf_full, cloudFull, true);
// bug in PCL - the binary file is not created with the good permissions!
char bufsys[256];
sprintf(bufsys, "chmod a+rw %s", buf_full);
system(bufsys);
}
}
//+----------------------------------------------------------------------------+
//| main() |
//-----------------------------------------------------------------------------+
int
main(int argc, char** argv)
{
common::PathFileString pfs(argv[0]);
//////////////////////////////////////////////////////////////////////////////
// User command line parser
AnyOption *opt = new AnyOption();
opt->autoUsagePrint(true);
opt->addUsage( "" );
opt->addUsage( "Usage: " );
char buff[256];
sprintf(buff, " Example: %s -r example1_noisy.cfg", pfs.getFileName().c_str());
opt->addUsage( buff );
opt->addUsage( " -h --help Prints this help" );
opt->addUsage( " -r --readfile <filename> Reads the polyhedra description file" );
opt->addUsage( " -t --gltopic <topic name> (opt) ROS Topic to send OpenGL commands " );
opt->addUsage( "" );
opt->setFlag( "help", 'h' );
opt->setOption( "readfile", 'r' );
opt->processCommandArgs( argc, argv );
if( opt->getFlag( "help" ) || opt->getFlag( 'h' ) ) {opt->printUsage(); delete opt; return(1);}
std::string gltopic("OpenGLRosComTopic");
if( opt->getValue( 't' ) != NULL || opt->getValue( "gltopic" ) != NULL ) gltopic = opt->getValue( 't' );
std::cerr << "[OpenGL communication topic is set to : \"" << gltopic << "\"]\n";
std::string readfile;
if( opt->getValue( 'r' ) != NULL || opt->getValue( "readfile" ) != NULL )
{
readfile = opt->getValue( 'r' );
std::cerr << "[ World description is read from \"" << readfile << "\"]\n";
}
else{opt->printUsage(); delete opt; return(-1);}
delete opt;
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Initialize ROS and OpenGLRosCom node (visualization)
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]"<< std::endl;
ros::init(argc, argv, pfs.getFileName().c_str());
if(ros::isInitialized())
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[OK]\n"<< std::endl;
else{
std::cerr << "["<<pfs.getFileName()<<"]:" << "[Initializing ROS]:[FAILED]\n"<< std::endl;
return(1);
}
COpenGLRosCom glNode;
glNode.CreateNode(gltopic);
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Reading the input file
std::cerr << "Reading the input file..." << std::endl;
ShapeVector shapes;
PoseVector poses;
IDVector ID;
if(ReadPolyhedraConfigFile(readfile, shapes, poses, ID)==false)
{
std::cerr << "["<<pfs.getFileName()<<"]:" << "Error reading file \"" << readfile << "\"\n";
return(-1);
}
std::cerr << "Read " << poses.size() << " poses of " << shapes.size() << " shapes with " << ID.size() << " IDs.\n";
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Visualizing the configuration of objects
std::cerr << "Visualizing the configuration of objects..." << std::endl;
for(unsigned int i=0; i<shapes.size(); i++)
{
for(size_t j=0; j<shapes[i].F.size(); j++)
{
glNode.LineWidth(1.0f);
glNode.AddColor3(0.3f, 0.6f, 0.5f);
glNode.AddBegin("LINE_LOOP");
for(size_t k=0; k<shapes[i].F[j].Idx.size(); k++)
{
int idx = shapes[i].F[j].Idx[k];
Eigen::Matrix<Real,3,1> Vt = poses[i]*shapes[i].V[ idx ];
glNode.AddVertex(Vt.x(), Vt.y(), Vt.z());
}
glNode.AddEnd();
}
}
glNode.SendCMDbuffer();
ros::spinOnce();
common::PressEnterToContinue();
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Running PROMTS with A* search algorithm
std::cerr << "Running PROMTS with Depth Limited search algorithm..." << std::endl;
promts::ProblemDataStruct pds("Depth-Limited Search");
pds.m_glNodePtr = &glNode;
PoseVector refined_poses(shapes.size());
refinePoses_DLSearch(shapes, poses, ID, refined_poses, pds);
//
//////////////////////////////////////////////////////////////////////////////
return(0);
}
