void
UniformSamplingExtractor<PointT>::filterPlanar (const PointInTPtr & input, std::vector<int> &kp_idx)
{
//create a search object
typename pcl::search::Search<PointT>::Ptr tree;
if (input->isOrganized () && !force_unorganized_)
tree.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
tree.reset (new pcl::search::KdTree<PointT> (false));
tree->setInputCloud (input);
size_t kept = 0;
for (size_t i = 0; i < kp_idx.size (); i++)
{
std::vector<int> neighborhood_indices;
std::vector<float> neighborhood_dist;
if (tree->radiusSearch (kp_idx[i], radius_, neighborhood_indices, neighborhood_dist))
{
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
EIGEN_ALIGN16 Eigen::Vector3f eigenValues;
EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors;
//compute planarity of the region
computeMeanAndCovarianceMatrix (*input, neighborhood_indices, covariance_matrix, xyz_centroid);
pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues);
float eigsum = eigenValues.sum ();
if (!pcl_isfinite(eigsum))
PCL_ERROR("Eigen sum is not finite\n");
float t_planar = threshold_planar_;
if(z_adaptative_)
t_planar *= (1 + (std::max(input->points[kp_idx[i]].z,1.f) - 1.f));
//if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2))
if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > t_planar))
{
//region is not planar, add to filtered keypoint
kp_idx[kept] = kp_idx[i];
kept++;
}
}
}
kp_idx.resize (kept);
}
pcl::PointCloud<pcl::Normal>::Ptr don(
pcl::PointCloud<PointT> & cloud,
pcl::PointCloud<NormalT> & normals, float radius1 = 0.2,
float radius2 = 0.5){
pcl::PointCloud<pcl::Normal>::Ptr donormals;
donormals.reset(new pcl::PointCloud<pcl::Normal>());
donormals->resize(cloud.size());
typename pcl::search::Search<PointT>::Ptr tree;
tree.reset (new pcl::search::KdTree<PointT> (false));
typename pcl::PointCloud<PointT>::ConstPtr cptr(&cloud, boost::serialization::null_deleter());
tree->setInputCloud(cptr);
std::vector<int> idxs;
std::vector<float> sq_dists;
auto avg_normal = [] (std::vector<int> idxs, pcl::PointCloud<NormalT> & normals) {
pcl::Normal sum;
for(int idx : idxs) {
sum.getNormalVector3fMap() += normals[idx].getNormalVector3fMap();
}
sum.getNormalVector3fMap() /= idxs.size();
return sum;
};
for(uint idx = 0; idx < cloud.size(); idx++){
std::vector<float> rads;
rads.push_back(radius1);
rads.push_back(radius2);
std::vector<pcl::Normal> avgs;
for(float rad : rads){
idxs.clear();
sq_dists.clear();
tree->radiusSearch(idx, rad, idxs, sq_dists);
idxs.push_back(idx);
avgs.push_back(avg_normal(idxs, normals));
}
(*donormals)[idx].getNormalVector3fMap() = (avgs[0].getNormalVector3fMap() - avgs[1].getNormalVector3fMap())/2.0;
}
return donormals;
}