int
main (int, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1)
{
std::cout<<"Couldn't read the file "<<argv[1]<<std::endl;
return (-1);
}
std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Estimated the normals" << std::endl;
// Creating the kdtree object for the search method of the extraction
boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
tree_ec->setInputCloud (cloud_ptr);
// Extracting Euclidean clusters using cloud and its normals
std::vector<int> indices;
std::vector<pcl::PointIndices> cluster_indices;
const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
const unsigned int min_cluster_size = 50;
pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
// Saving the clusters in seperate pcd files
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_ptr->points[*pit]);
cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "./cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
return (0);
}
inline void
filter_depth_discontinuity(
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int neighbors = 2,
float threshold = 0.3,
float distance_min = 1,
float distance_max = 300,
float epsilon = 0.5
)
{
//std::cout << "neigbors " << neighbors << " epsilon " << epsilon << " distance_max " << distance_max <<std::endl;
boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n( new pcl::search::KdTree<PointT>() );
tree_n->setInputCloud(in.makeShared());
tree_n->setEpsilon(epsilon);
for(int i = 0; i< in.points.size(); ++i){
std::vector<int> k_indices;
std::vector<float> square_distance;
if ( in.points.at(i).z > distance_max || in.points.at(i).z < distance_min) continue;
//Position in image is known
tree_n->nearestKSearch(in.points.at(i), neighbors, k_indices, square_distance);
//std::cout << "hier " << i << " z " << in.points.at(i).z <<std::endl;
#ifdef USE_SQUERE_DISTANCE
const float point_distance = distance_to_origin<PointT>(in.points.at(i));
#else
const float point_distance = in.points.at(i).z;
#endif
float value = 0; //point_distance;
unsigned int idx = 0;
for(int n = 0; n < k_indices.size(); ++n){
#ifdef USE_SQUERE_DISTANCE
float distance_n = distance_to_origin<PointT>(in.points.at(k_indices.at(n)));
#else
float distance_n = in.points.at(k_indices.at((n))).z;
#endif
if(value < distance_n - point_distance){
idx = k_indices.at(n);
value = distance_n - point_distance;
}
}
if(value > threshold){
out.push_back(in.points.at(i));
out.at(out.size()-1).intensity = sqrt(value);
}
}
}
int
main (int, char** av)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(av[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << av[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << av[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setSmoothModeFlag (false); // Depends on the cloud being processed
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
// Writing the resulting cloud into a pcd file
std::cout << "No of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
return (0);
}
int
main(int, char** argv)
{
std::string filename = argv[1];
std::cout << "Reading " << filename << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
{
PCL_ERROR ("Couldn't read file");
return -1;
}
std::cout << "points: " << cloud_xyz->points.size () <<std::endl;
// Parameters for sift computation
const float min_scale = 0.01f;
const int n_octaves = 3;
const int n_scales_per_octave = 4;
const float min_contrast = 0.001f;
// Estimate the normals of the cloud_xyz
pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setInputCloud(cloud_xyz);
ne.setSearchMethod(tree_n);
ne.setRadiusSearch(0.2);
ne.compute(*cloud_normals);
// Copy the xyz info from cloud_xyz and add it to cloud_normals as the xyz field in PointNormals estimation is zero
for(size_t i = 0; i<cloud_normals->points.size(); ++i)
{
cloud_normals->points[i].x = cloud_xyz->points[i].x;
cloud_normals->points[i].y = cloud_xyz->points[i].y;
cloud_normals->points[i].z = cloud_xyz->points[i].z;
}
// Estimate the sift interest points using normals values from xyz as the Intensity variants
pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
pcl::PointCloud<pcl::PointWithScale> result;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
sift.setSearchMethod(tree);
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
sift.setMinimumContrast(min_contrast);
sift.setInputCloud(cloud_normals);
sift.compute(result);
std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl;
/*
// Copying the pointwithscale to pointxyz so as visualize the cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
copyPointCloud(result, *cloud_temp);
std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl;
// Visualization of keypoints along with the original cloud
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0);
viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");
viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
while(!viewer.wasStopped ())
{
viewer.spinOnce ();
}
*/
return 0;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
struct timeval tpstart,tpend;
float timeuse;
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(argv[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << argv[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << argv[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
gettimeofday(&tpstart,NULL);
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setKSearch(30);
//ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setMinClusterSize (50);
rg.setSmoothModeFlag (true); // Depends on the cloud being processed
rg.setSmoothnessThreshold (10*M_PI/180);
rg.setResidualTestFlag (true);
rg.setResidualThreshold (0.005);
rg.setCurvatureTestFlag (true);
rg.setCurvatureThreshold (0.008);
rg.setNumberOfNeighbours (30);
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
cloud_segmented->width = cloud_segmented->points.size();
cloud_segmented->height = 1;
gettimeofday(&tpend,NULL);
timeuse=1000000*(tpend.tv_sec-tpstart.tv_sec) + tpend.tv_usec-tpstart.tv_usec;
timeuse/=1000000;
std::cout << "Segmentation time: " << timeuse << std::endl;
std::cout << "Number of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
std::cout << "Number of points in the segments: " << cloud_segmented->size() << std::endl;
pcl::visualization::PCLVisualizer viewer ("Detected planes with Pseudo-color");
viewer.setBackgroundColor (1.0, 1.0, 1.0);
viewer.addPointCloud (cloud_segmented, "cloud segmented", 0);
viewer.addCoordinateSystem ();
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud segmented");
viewer.spin();
// Writing the resulting cloud into a pcd file
return (0);
}
