Normal Estimation

Normal Estimation

pcl::PointCloud<pcl::Normal>::Ptr normals_out (new pcl::PointCloud<pcl::Normal>);

pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> norm_est;

// Use a FLANN-based KdTree to perform neighborhood searches
norm_est.setSearchMethod (pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr
(new pcl::KdTreeFLANN<pcl::PointXYZRGB>));

// Specify the size of the local neighborhood to use when 
// computing the surface normals
norm_est.setRadiusSearch (normal_radius);

// Set the input points
norm_est.setInputCloud (points);

// Set the search surface (i.e., the points that will be used 
// when search for the input points’ neighbors) 
norm_est.setSearchSurface (points);

// Estimate the surface normals and 
// store the result in "normals_out" 
norm_est.compute (*normals_out);

Random Sample Consensus (RANSAC)

• RANSAC is a randomized algorithm for robust model fitting.

• Its basic operation

  • select sample set - 2 points
  • compute model - line equation
  • compute and count inliers - e.g. epsilon-band
  • repeat until sufficiently confident -e.g. 95%

• Use normal for plane fitting

Plane RANSAC

// necessary includes
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>

// ...
// Create a shared plane model pointer directly
SampleConsensusModelNormalPlane<PointXYZ, pcl::Normal>::Ptr model (new SampleConsensusModelNormalPlane<PointXYZ, pcl::Normal> (input));

// Set normals
model->setInputNormals(normals);

// Set the normal angular distance weight.
model->setNormalDistanceWeight(0.5f);

// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);

// perform the segmenation step
bool result = sac.computeModel ();

// get inlier indices
boost::shared_ptr<vector<int> > inliers (new vector<int>); sac.getInliers (*inliers);
cout << "Found model with " << inliers->size () << " inliers";

// get model coefficients
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
cout << ", plane normal is: " << coeff[0] << ", " << coeff[1] << ", " << coeff[2] << "." << en

/*---optional---*/
// perform a refitting step
Eigen::VectorXf coeff_refined; 
model->optimizeModelCoefficients (*inliers, coeff, coeff_refined); 
model->selectWithinDistance (coeff_refined, 0.03, *inliers);
cout << "After refitting, model contains " << inliers->size () << " inliers"; cout << ", plane normal is: " << coeff_refined[0] << ", " << coeff_refined[1] << ", " << coeff_refined[2] << "." << endl; 

// Projection
PointCloud<PointXYZ> proj_points;
model->projectPoints (*inliers, coeff_refined, proj_points);

Clustering

• ExtractPolygonalPrismData

// Create a Convex Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);

pcl::ConvexHull<pcl::PointXYZ> chull; 
chull.setInputCloud (inliers_cloud); 
chull.reconstruct (*cloud_hull);

// segment those points that are in the polygonal prism
ExtractPolygonalPrismData<PointXYZ> ex; 
ex.setInputCloud (outliers); 
ex.setInputPlanarHull (cloud_hull);
PointIndices::Ptr output (new PointIndices); 
ex.segment (*output);

// Create EuclideanClusterExtraction and set parameters
pcl::EuclideanClusterExtraction<PointT> ec; 
ec.setClusterTolerance (cluster_tolerance); 
ec.setMinClusterSize (min_cluster_size); 
ec.setMaxClusterSize (max_cluster_size);

// set input cloud and let it run
ec.setInputCloud (input); 
ec.extract (cluster_indices_out);