点云密度粗估计

int caculateDensity(std::vector<PointType>& in_points, double & density, const int& k) {

    if (in_points.size()<k)
    {
        std::cout << " erorr  -----  in_points.size() < k " << std::endl;
        return -1;
    }

    /******************点云密度粗估计****************************/
    KDT::KDTree kdtree;
    kdtree.setInputPointCloud(in_points);
    kdtree.setNumOfLeafData(50);
    kdtree.buildKDTree();
    PointType search_point;    //存储搜索点

    std::vector<double> dis_vect;
    for (size_t i = 0; i < in_points.size(); i = i + k)
    {
        std::vector<size_t>searchIndex(k);
        std::vector<float> searchDistance(k);
        kdtree.runKNNSearchK(in_points[i], k, &searchIndex[0], &searchDistance[0]);

        double dis = 0.0;
        for (size_t j = 1; j < searchDistance.size(); j++)
        {
            dis += searchDistance[j];
        }
        dis = dis / (searchDistance.size() - 1);
        dis_vect.push_back(dis);
    }

    density = 0.0;
    for (size_t i = 0; i < dis_vect.size(); i++)
    {
        density += dis_vect[i];
    }
    density = density / dis_vect.size();

    return 0;
}