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);
}
예제 #2
0
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;
}