int
main (int argc, char** argv)
{
std::string filename = argv[1];
std::cout << "Reading " << filename << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file");
return (-1);
}
std::cout << "Loaded " << cloud->points.size () << " points." << std::endl;
// Compute the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
normal_estimation.setInputCloud (cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
normal_estimation.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::Normal>);
normal_estimation.setRadiusSearch (0.03);
normal_estimation.compute (*cloud_with_normals);
// Setup the principal curvatures computation
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> principal_curvatures_estimation;
// Provide the original point cloud (without normals)
principal_curvatures_estimation.setInputCloud (cloud);
// Provide the point cloud with normals
principal_curvatures_estimation.setInputNormals (cloud_with_normals);
// Use the same KdTree from the normal estimation
principal_curvatures_estimation.setSearchMethod (tree);
principal_curvatures_estimation.setRadiusSearch (1.0);
// Actually compute the principal curvatures
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
principal_curvatures_estimation.compute (*principal_curvatures);
std::cout << "output points.size (): " << principal_curvatures->points.size () << std::endl;
// Display and retrieve the shape context descriptor vector for the 0th point.
pcl::PrincipalCurvatures descriptor = principal_curvatures->points[0];
std::cout << descriptor << std::endl;
return 0;
}
void FeatureEval::curvature(){
boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
if(_cloud == nullptr)
return;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr filt_cloud;
std::vector<int> big_to_small;
filt_cloud = downsample(_cloud, subsample_res_, big_to_small);
// Compute
t_.start(); qDebug() << "Timer started (Curvature)";
pcl::PrincipalCurvaturesEstimation<pcl::PointXYZINormal, pcl::PointXYZINormal, pcl::PrincipalCurvatures> principal_curvatures_estimation;
principal_curvatures_estimation.setInputCloud (filt_cloud);
principal_curvatures_estimation.setInputNormals (filt_cloud);
pcl::search::KdTree<pcl::PointXYZINormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZINormal>);
principal_curvatures_estimation.setSearchMethod (tree);
principal_curvatures_estimation.setRadiusSearch (search_radius_);
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
principal_curvatures_estimation.compute (*principal_curvatures);
qDebug() << "Curvature in " << (time_ = t_.elapsed()) << " ms";
// Correlate
size_t csize = sizeof(pcl::PrincipalCurvatures)/sizeof(float);
computeCorrelation(reinterpret_cast<float*>(principal_curvatures->points.data()), 2, principal_curvatures->points.size(), big_to_small, csize, 3);
if(!visualise_on_) return;
// Draw
int w = _cloud->scan_width();
int h = _cloud->scan_height();
boost::shared_ptr<std::vector<float>> grid = boost::make_shared<std::vector<float>>(w*h, 0.0f);
const std::vector<int> & cloudtogrid = _cloud->cloudToGridMap();
for(uint big_idx = 0; big_idx < _cloud->size(); big_idx++) {
int small_idx = big_to_small[big_idx];
int grid_idx = cloudtogrid[big_idx];
pcl::PrincipalCurvatures & pc = (*principal_curvatures)[small_idx];
(*grid)[grid_idx] = pc.pc1 + pc.pc2;
}
drawFloats(grid, _cloud);
}
