int main( int argc, char ** argv ) {
pcl::console::setVerbosityLevel( pcl::console::L_ERROR );
if ( argc < 2 ) {
print_usage( argv[0] );
return (-1);
}
if (pcl::console::find_switch( argc, argv, "-help" ) ||
pcl::console::find_switch( argc, argv, "-h" )) {
print_usage( argv[0] );
}
// load images
std::string rgbp = argv[1], depthp = argv[2];
cv::Mat rgbimg = cv::imread( rgbp, CV_LOAD_IMAGE_COLOR ),
depthimg = cv::imread( depthp, CV_LOAD_IMAGE_ANYDEPTH );
float param[4] = { 319.5, 239.5, 525.0, 525.0 };
// convert to point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud( new pcl::PointCloud<pcl::PointXYZRGB>() );
cloud = img2cloud( rgbimg, depthimg, param );
pcl::visualization::PCLVisualizer::Ptr viewer( new pcl::visualization::PCLVisualizer("viewer") );
viewer->setBackgroundColor( 0, 0, 0 );
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "cloud");
viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud" );
viewer->addCoordinateSystem( 1.0 );
viewer->initCameraParameters();
while (!viewer->wasStopped ()) {
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
iss3d(cloud);
harris6d( cloud );
harris3d( cloud, pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::HARRIS );
harris3d( cloud, pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::TOMASI );
harris3d( cloud, pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::LOWE );
harris3d( cloud, pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::NOBLE );
harris3d( cloud, pcl::HarrisKeypoint3D<pcl::PointXYZRGB, pcl::PointXYZI>::CURVATURE );
return 1;
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
double normals_radius= 0.005;
if (pcl::console::parse (argc, argv, "-n", normals_radius) >= 0)
{
std::cout << " Radius: " << normals_radius << "\n";
}
double harris_radius = 0.5;
if (pcl::console::parse (argc, argv, "--hariss_radius", harris_radius) >= 0)
{
std::cout << " harris_radius: " << harris_radius << "\n";
}
double harris_search_radius = 0.01;
if (pcl::console::parse (argc, argv, "--harris_search_radius", harris_search_radius) >= 0)
{
std::cout << " harris_search_radius: " << harris_search_radius << "\n";
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[1], *cloudRGB) == -1) //* load the file
{
std::cout << "Couldn't read file cloud.pcd ";
return (-1);
}
pcl::copyPointCloud(*cloudRGB, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr harris3d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr iss3d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr susan (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr harris6d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sift (new pcl::PointCloud<pcl::PointXYZ>);
NormalCloudPtr normals(new NormalCloud());
pcl::NormalEstimationOMP<PointT, NormalT> est;
est.setRadiusSearch(normals_radius);
est.setInputCloud(cloud);
est.compute(*normals);
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints_temp(
new pcl::PointCloud<pcl::PointXYZI>());
HarrisKeypoint* detector = new HarrisKeypoint(HarrisKeypoint::HARRIS);
detector->setNonMaxSupression(true);
detector->setRadius(harris_radius);
detector->setRadiusSearch(harris_search_radius);
detector->setMethod(HarrisKeypoint::HARRIS);
// detector->setKSearch();
detector->setNumberOfThreads(10);
detector->setSearchMethod(tree);
// detector->setThreshold();
// detector->use_indices_ = false;
detector->setInputCloud(cloud);
detector->compute(*keypoints_temp);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints(
new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*keypoints_temp, *harris3d);
}
std::cout << " harris3d size :" << harris3d->size() << std::endl;;
/*
{
double iss_salient_radius_;
double iss_non_max_radius_;
double iss_gamma_21_ (0.975);
double iss_gamma_32_ (0.975);
double iss_min_neighbors_ (20);
int iss_threads_ (10);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
// Fill in the model cloud
double model_resolution = 0.01;
// Compute model_resolution
iss_salient_radius_ = 6 * model_resolution;
iss_non_max_radius_ = 4 * model_resolution;
//
// Compute keypoints
//
pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
iss_detector.setSearchMethod (tree);
iss_detector.setSalientRadius (iss_salient_radius_);
iss_detector.setNonMaxRadius (iss_non_max_radius_);
iss_detector.setThreshold21 (iss_gamma_21_);
iss_detector.setThreshold32 (iss_gamma_32_);
iss_detector.setMinNeighbors (iss_min_neighbors_);
iss_detector.setNumberOfThreads (iss_threads_);
iss_detector.setInputCloud (cloud);
iss_detector.compute (*iss3d);
}
std::cout << " iss3d size :" << iss3d->size() << std::endl;
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::SUSANKeypoint<pcl::PointXYZRGB, pcl::PointXYZRGB>* susan3D = new pcl::SUSANKeypoint<pcl::PointXYZRGB, pcl::PointXYZRGB>;
susan3D->setInputCloud(cloudRGB);
susan3D->setNonMaxSupression(true);
susan3D->setSearchMethod(tree);
susan3D->setRadius(harris_radius);
susan3D->setRadiusSearch(harris_search_radius);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr keypoints (new pcl::PointCloud<pcl::PointXYZRGB> ());
susan3D->compute(*keypoints);
pcl::PointCloud<pcl::PointXYZ>::Ptr susan(new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*keypoints, *susan);
}
std::cout << " susan size :" << susan->size() << std::endl;
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints_temp(
new pcl::PointCloud<pcl::PointXYZI>());
HarrisKeypoint6D* detector = new HarrisKeypoint6D(HarrisKeypoint::HARRIS);
detector->setNonMaxSupression(true);
detector->setRadius(harris_radius);
detector->setRadiusSearch(harris_search_radius);
// detector->setMethod(HarrisKeypoint::HARRIS);
detector->setNumberOfThreads(10);
detector->setSearchMethod(tree);
// detector->setThreshold();
// detector->use_indices_ = false;
detector->setInputCloud(cloudRGB);
detector->compute(*keypoints_temp);
pcl::copyPointCloud(*keypoints_temp, *harris6d);
}
std::cout << " harris6d size :" << harris6d->size() << std::endl;
/*
*/
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> white (cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red (harris3d, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green (iss3d, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue (susan, 0, 0, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> col2 (harris6d, 255, 0, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> col3 (sift, 255, 255, 0);
//////
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ> (cloud, white, "cloud");
viewer->addPointCloud<pcl::PointXYZ> (harris3d, red, "keypoints2");
viewer->addPointCloud<pcl::PointXYZ> (iss3d, green, "keypoints3");
viewer->addPointCloud<pcl::PointXYZ> (susan, blue, "keypoint4s");
viewer->addPointCloud<pcl::PointXYZ> (harris6d, col2, "keypoin3ts");
viewer->addPointCloud<pcl::PointXYZ> (sift, col3, "keypoint36");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints2");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints3");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoint4s");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoin3ts");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoint36");
// viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 5, 0.05, "normals");
viewer->addCoordinateSystem (1.0, "global");
viewer->initCameraParameters ();
// viewer->addSphere (cloud->points[0], normals_radius, "sphere+norm", 0);
// viewer->addSphere (cloud->points[0], shot_radius, "sphere+shot", 0);
//--------------------
// -----Main loop-----
//--------------------
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
