template <typename PointT> void
pcl::UnaryClassifier<PointT>::segment (pcl::PointCloud<pcl::PointXYZRGBL>::Ptr &out)
{
if (trained_features_.size () > 0)
{
// convert cloud into cloud with XYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
convertCloud (input_cloud_, tmp_cloud);
// compute FPFH feature histograms for all point of the input point cloud
pcl::PointCloud<pcl::FPFHSignature33>::Ptr input_cloud_features (new pcl::PointCloud<pcl::FPFHSignature33>);
computeFPFH (tmp_cloud, input_cloud_features, normal_radius_search_, fpfh_radius_search_);
// query the distances from the input data features to all trained features
std::vector<int> indices;
std::vector<float> distance;
queryFeatureDistances (trained_features_, input_cloud_features, indices, distance);
// assign a label to each point of the input point cloud
int n_feature_means = static_cast<int> (trained_features_[0]->points.size ());
convertCloud (input_cloud_, out);
assignLabels (indices, distance, n_feature_means, feature_threshold_, out);
//std::cout << "Assign labels - DONE" << std::endl;
}
else
PCL_ERROR ("no training features set \n");
}
template <typename PointT> void
pcl::UnaryClassifier<PointT>::train (pcl::PointCloud<pcl::FPFHSignature33>::Ptr &output)
{
// convert cloud into cloud with XYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
convertCloud (input_cloud_, tmp_cloud);
// compute FPFH feature histograms for all point of the input point cloud
pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>);
computeFPFH (tmp_cloud, feature, normal_radius_search_, fpfh_radius_search_);
//PCL_INFO ("Number of input cloud features: %d\n", static_cast<int> (feature->points.size ()));
// use k-means to cluster the features
kmeansClustering (feature, output, cluster_size_);
}
V4R_EXPORTS
void computeNormals(const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
pcl::PointCloud<pcl::Normal>::Ptr &normals,
int method, float radius)
{
CHECK(normals);
if(method == 0)
{
pcl::NormalEstimation<PointT, pcl::Normal> n3d;
n3d.setRadiusSearch (radius);
n3d.setInputCloud (cloud);
n3d.compute (*normals);
}
else if(method == 1)
{
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(15.f);//20.0f);
ne.setDepthDependentSmoothing(false);//param.normals_depth_dependent_smoothing);
ne.setInputCloud(cloud);
ne.setViewPoint(0,0,0);
ne.compute(*normals);
}
else if(method == 2)
{
pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;
ne.setRadiusSearch ( radius );
ne.setInputCloud ( cloud );
ne.compute ( *normals );
}
else if(method == 3)
{
ZAdaptiveNormals::Parameter n_param;
n_param.adaptive = true;
ZAdaptiveNormals nest(n_param);
DataMatrix2D<Eigen::Vector3f>::Ptr kp_cloud( new DataMatrix2D<Eigen::Vector3f>() );
DataMatrix2D<Eigen::Vector3f>::Ptr kp_normals_tmp( new DataMatrix2D<Eigen::Vector3f>() );
convertCloud(*cloud, *kp_cloud);
nest.compute(*kp_cloud, *kp_normals_tmp);
convertNormals(*kp_normals_tmp, *normals);
}
else if(method==4)
{
boost::shared_ptr<PreProcessorAndNormalEstimator<PointT, pcl::Normal> > normal_estimator;
normal_estimator.reset (new PreProcessorAndNormalEstimator<PointT, pcl::Normal>);
normal_estimator->setCMR (false);
normal_estimator->setDoVoxelGrid (false);
normal_estimator->setRemoveOutliers (false);
normal_estimator->setValuesForCMRFalse (0.003f, 0.02f);
normal_estimator->setForceUnorganized(true);
typename pcl::PointCloud<PointT>::Ptr processed (new pcl::PointCloud<PointT>);
normal_estimator->estimate (cloud, processed, normals);
}
else
{
throw std::runtime_error("Chosen normal computation method not implemented!");
}
// Normalize normals to unit length
for ( size_t normal_pt_id = 0; normal_pt_id < normals->points.size(); normal_pt_id++)
{
Eigen::Vector3f n1 = normals->points[normal_pt_id].getNormalVector3fMap();
n1.normalize();
normals->points[normal_pt_id].normal_x = n1(0);
normals->points[normal_pt_id].normal_y = n1(1);
normals->points[normal_pt_id].normal_z = n1(2);
}
}
