void MOOSAppDocumentation::showTitleSection(string title_section)
{
blk(" ");
blu("================================================================");
blu((" " + m_moosapp_name + " - " + title_section).c_str());
blu("================================================================");
blk(" ");
}
int main(int argc, char** argv)
{
boost::program_options::variables_map vm;
if (!parseCommandLine(argc, argv, vm))
return 0;
const float radius_nms = vm["radiusNMS"].as<float>();
const float radius_features = vm["radiusFeatures"].as<float>();
const float threshold = vm["threshold"].as<float>();
const std::string path_rf = vm["pathRF"].as<std::string>();
const std::string path_cloud = vm["pathCloud"].as<std::string>();
//create detector
pcl::keypoints::KeypointLearningDetector<PointInT, KeypointT>::Ptr detector(new pcl::keypoints::KeypointLearningDetector<PointInT, KeypointT>());
detector->setNAnnulus(ANNULI);
detector->setNBins(BINS);
detector->setNonMaxima(true);
detector->setNonMaxRadius(radius_nms);
detector->setNonMaximaDrawsRemove(false);
detector->setPredictionThreshold(threshold);
detector->setRadiusSearch(radius_features); //40mm change according to the unit of measure used in your point cloud
if (detector->loadForest(path_rf))
{
std::cout << "Detector created." << std::endl;
}
else
{
return -1;
}
//load and subsample point cloud
pcl::PointCloud<PointInT>::Ptr cloud(new pcl::PointCloud<PointInT>());
pcl::io::loadPCDFile(path_cloud, *cloud);
pcl::UniformSampling<PointInT>::Ptr source_uniform_sampling(new pcl::UniformSampling<PointInT>());
bool sub_sampling = vm.count("subSampling") > 0;
float leaf = 0.0;
if(sub_sampling)
{
leaf = vm["leaf"].as<float>();
source_uniform_sampling->setRadiusSearch(leaf);
source_uniform_sampling->setInputCloud(cloud);
source_uniform_sampling->filter(*cloud);
}
std::cout << "Point cloud loaded" << std::endl;
//Compute normals
pcl::NormalEstimation<PointInT, PointNormalT> ne;
pcl::PointCloud<PointNormalT>::Ptr normals(new pcl::PointCloud<PointNormalT>);
ne.setInputCloud(cloud);
pcl::search::KdTree<PointInT>::Ptr kdtree(new pcl::search::KdTree<PointInT>());
ne.setKSearch(10);
ne.setSearchMethod(kdtree);
ne.compute(*normals);
std::cout << "Normals Computed" << std::endl;
//Set true with laser scanner dataset
bool flip_normals = vm.count("flipNormals") > 0;
if (flip_normals)
{
std::cout << "Flipping "<< std::endl;
//Sensor origin is inside the model, flip normals to make normal sign coherent with scenes
std::transform(normals->points.begin(), normals->points.end(), normals->points.begin(), [](pcl::Normal p) -> pcl::Normal {auto q = p; q.normal[0] *= -1; q.normal[1] *= -1; q.normal[2] *= -1; return q; });
}
//setup the detector
detector->setInputCloud(cloud);
detector->setNormals(normals);
//detect keypoints
pcl::PointCloud<KeypointT>::Ptr keypoint(new pcl::PointCloud<KeypointT>());
detector->compute(*keypoint);
std::cout << "Keypoint computed" << std::endl;
//show results
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer.reset(new pcl::visualization::PCLVisualizer);
viewer->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<PointInT> blu(cloud, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloud, blu,"model cloud");
pcl::visualization::PointCloudColorHandlerCustom<KeypointT> red(keypoint, 255, 0, 0);
viewer->addPointCloud<KeypointT>(keypoint, red, "Keypoint");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 6, "Keypoint");
std::cout << "viewer ON" << std::endl;
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
viewer->removeAllPointClouds();
std::cout << "DONE" << std::endl;
if (vm.count("pathKP"))
{
const std::string path = vm["pathKP"].as<std::string>();
pcl::io::savePCDFileASCII<KeypointT>(path, *keypoint);
}
}
