int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//load point cloud
loadcloud(cloud_ptr);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
//calculate the normal of the point cloud.
NormalCalculation(cloud_ptr,normals);
//display point cloud.
displaycloud(cloud_ptr);
//display the normal of the point cloud.
displayNormal(cloud_ptr, normals);
//segmentation by region growing method.
segmented_cloud=RegionGrowingSegmentation(cloud_ptr,normals);
//display the segmented the cloud.
displaycloud(segmented_cloud);
return 0;
}
pair<float,int> getSmallestDistance (const pcl::PointCloud<PointT> &cloud1,const pcl::PointCloud<PointT> &cloud2)
{
float min_distance = FLT_MAX;
int min_index = 0;
pcl::PointCloud<PointT>::Ptr small_cloud;
pcl::PointCloud<PointT>::Ptr big_cloud;
if (cloud1.points.size() > cloud2.points.size()){
pcl::PointCloud<PointT>::Ptr cloud_ptr1(new pcl::PointCloud<PointT > (cloud1));
pcl::PointCloud<PointT>::Ptr cloud_ptr2(new pcl::PointCloud<PointT > (cloud2));
small_cloud = cloud_ptr2;
big_cloud = cloud_ptr1;
}else {
pcl::PointCloud<PointT>::Ptr cloud_ptr1(new pcl::PointCloud<PointT > (cloud1));
pcl::PointCloud<PointT>::Ptr cloud_ptr2(new pcl::PointCloud<PointT > (cloud2));
small_cloud = cloud_ptr1;
big_cloud = cloud_ptr2;
}
pcl::KdTreeFLANN<PointT>::Ptr tree (new pcl::KdTreeFLANN<PointT>);//= boost::make_shared<pcl::KdTreeFLANN<PointT> > ();
//initTree (0, tree);
tree->setInputCloud (big_cloud );// ,indicesp);
std::vector<int> nn_indices;
nn_indices.resize(2);
std::vector<float> nn_distances;
nn_distances.resize(2);
//float tolerance = 0.3;
for (size_t i = 0; i < small_cloud->points.size (); ++i)
{
//if (!tree->radiusSearch ((*small_cloud).points[i], tolerance, nn_indices, nn_distances))
tree->nearestKSearch (small_cloud->points[i], 2 , nn_indices, nn_distances);
for (size_t j = 0; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
{
//float distance = pow(cloud1.points[i].x - cloud2.points[nn_indices[j]].x,2) + pow(cloud1.points[i].y - cloud2.points[ nn_indices[j]].y,2) + pow(cloud1.points[i].z - cloud2.points[ nn_indices[j]].z,2);
//cout<< "i,j = " << i << "," << j<< " dist = " <<distance << endl;
//float a = nn_distances[j];
//cout<< nn_distances[j] << " "<< a << endl;
if (min_distance > nn_distances[j])
{
min_distance = nn_distances[j];
min_index = nn_indices[j];
// cout << "changing min_distance to " << min_distance<< endl;
}
}
}
/*
for (size_t i = 0; i < cloud1.points.size (); ++i)
{
for (size_t j = 0; j < cloud2.points.size (); ++j)
{
float distance = pow(cloud1.points[i].x - cloud2.points[j].x,2) + pow(cloud1.points[i].y - cloud2.points[j].y,2) + pow(cloud1.points[i].z - cloud2.points[j].z,2);
// cerr<< "i,j = " << i << "," << j<< " dist = " <<distance << endl;
if (min_distance > distance) min_distance = distance;
}
}
*/
return make_pair(sqrt(min_distance),min_index) ;
}
