XZ6_3(f,s)=XZ6(i,s);
end f=f+1;
end end
Par6_3Y = CircleFitByPratt(XZ6_3);
All.to n. 11 Codice PCL tubazioni corridoio
INPUT: Nuvola di punti del soffitto del corridoio in formato .pcd OUTPUT: 1) Nuvola di punti cluster in formato .pcd
2) Nuvola di punti dei cilindri in formato .pcd
---//Caricamento delle funzionalità necessarie//
include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <string>
#include <sstream>
--- //Definizione degli oggetti e delle variabili necessarie//
using namespace std;
typedef pcl::PointXYZ PointT;
int
main (int argc, char** argv) {
pcl::PCDWriter writer;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::search::Search<pcl::PointXYZ>::Ptr tree =
boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
179
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
pcl::ExtractIndices<PointT> extract;
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
pcl::PCDReader reader;
--- //Caricamento della nuvola di punti//
reader.read("corridoioF6completo.pcd", *cloud);
--- //Stima delle normali ai punti//
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch(40);
normal_estimator.compute (*normals);
--- //Settaggio parametri cluster//
reg.setMinClusterSize (500);
reg.setMaxClusterSize (10000000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (60);
reg.setInputCloud (cloud);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);
reg.setCurvatureThreshold (1);
--- //Estrazione dei cluster dalla nuvola di punti//
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
--- //Creazione dei singoli cluster//
cout << "Number of clusters is equal to " << clusters.size () << endl;
cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
cout << "These are the indices of the points of the initial" <<
endl << "cloud that belong to the first cluster:" << endl;
int counter = 0;
while (counter < clusters[0].indices.size ())
{
cout << clusters[0].indices[counter] << ", ";
counter++;
if (counter % 10 == 0)
cout << endl;
}
cout << endl;
--- //Salvataggio di ogni cluster in un file//
int j = 0;
for (vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) cloud_cluster->points.push_back(cloud->points[*pit]); //*
180
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << endl;
stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
j++;
}
string st1 = "cloud_cluster_";
string ext = ".pcd";
string filename;
for (int i = 0; i <= j; i++)
{
stringstream ss;
ss << i;
filename = st1 + ss.str() + ext;
cout << filename << "\n";
reader.read(filename, *cloud2);
cerr << "PointCloud has: " << cloud2->points.size() << " data points." << endl;
--- //Stima delle normali ai punti//
ne.setSearchMethod(tree);
ne.setInputCloud(cloud2);
ne.setKSearch(100);
ne.compute(*cloud_normals);
--- //Settaggio parametri modello planare//
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight(0.05);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud2);
seg.setInputNormals(cloud_normals);
--- //Ricerca dei punti della nuvola appartenenti al piano//
seg.segment(*inliers_plane, *coefficients_plane);
cerr << "Plane coefficients: " << *coefficients_plane << endl;
--- //Estrazione dei punti del piano dalla nuvola//
extract.setInputCloud(cloud2);
extract.setIndices(inliers_plane);
extract.setNegative(false);
extract.filter(*cloud_plane);
cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << "
data points." << endl;
if (cloud_plane->points.size() <= (cloud2->points.size()) / 2)
{
181
--- //Settaggio parametri modello cilindrico//
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_CYLINDER);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setNormalDistanceWeight(0.01);
seg.setMaxIterations(10000);
seg.setDistanceThreshold(0.1);
seg.setRadiusLimits(0, 0.2);
seg.setInputCloud(cloud2);
seg.setInputNormals(cloud_normals);
--- //Ricerca dei punti della nuvola appartenenti al cilindro//
seg.segment(*inliers_cylinder, *coefficients_cylinder);
cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
--- //Salvataggio del cilindro in un file .pcd//
extract.setInputCloud(cloud2);
extract.setIndices(inliers_cylinder);
extract.setNegative(false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
extract.filter(*cloud_cylinder);
cerr << "PointCloud representing the cylindrical component: " <<
cloud_cylinder->points.size() << " data points." << endl;
stringstream st;
st << "cloud_cylinder_" << i << ".pcd";
writer.write<pcl::PointXYZ>(st.str(), *cloud_cylinder, false);
}
--- //Non salva il cilindro perché è un piano//
else
{
cout << "E'un piano" << endl;
system("pause");
}
}
return (0);
}
COMMENTO:
Le due parti del codice eseguono le seguenti procedure:
la prima parte dell’algoritmo seleziona parti della nuvola e unisce i punti a distanza ravvicinata discriminandoli in base alla forma della superficie; ciò avviene confrontando gli angoli tra le normali alle superfici prese nell’intorno dei singoli punti. Viene denominato cluster un insieme di punti appartenenti ad una porzione di superficie omogeneamente liscia. Pertanto l'output di questa parte dell’algoritmo è il set di cluster. Queste informazioni sono organizzate in file di estensione .pcd.
nella seconda parte dell’algoritmo vengono utilizzati i file .pcd ottenuti precedentemente e vengono classificati in base a due forme geometriche semplici: i cilindri e i piani. Le
182
informazioni parametriche dei piani non vengono memorizzate, mentre l’output è rappresentato dai parametri dei cilindri stampati a schermo come nella prova n.4. Le nuvole sono salvate sottoforma di file con estensione .pcd.