FPS点云下采样

        Farthest Point Sampling (FPS)，也就是每次采样的时候都选择离之前采样得到的 k − 1 k-1 k−1个点距离最远的点。

        FPS的计算量与输入点云的点数呈平方相关。说明不适合用来处理大规模点云。大规模点云处理时，速度非常慢。但是FPS的下采样具有很好的保留点云的特征，常用于PointNet++, PointCNN等。

int main()
{
    std::string lasPath = "/Users/uchen/LasData/导线.las";
    LasIo io;
    PointCloud pc;
    io.GetLasPointCloud(lasPath, pc);
    auto newPc = pc.FarthestPointDownSample(1000);

    return 0;
}


std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(size_t numSamples) const
{
    if (numSamples > m_points.size() || numSamples <= 0)
    {
       return std::shared_ptr<PointCloud>();
    }

    std::vector<size_t> selectedIndices;
    selectedIndices.reserve(numSamples);
    const size_t numPoints = m_points.size();
    std::vector<double> distances(numPoints,
                                  std::numeric_limits<double>::infinity());
    size_t farthestIndex = 0;
    for (size_t i = 0; i < numSamples; i++)
    {
        selectedIndices.push_back(farthestIndex);
        const Eigen::Vector3d &selected = m_points[farthestIndex];
        double maxDist = 0;
        for (size_t j = 0; j < numPoints; j++)
        {
            double dist = (m_points[j] - selected).squaredNorm();
            distances[j] = std::min(distances[j], dist);
            if (distances[j] > maxDist)
            {
                maxDist = distances[j];
                farthestIndex = j;
            }
        }
    }

    auto newPc = std::make_shared<PointCloud>();
    for (int i = 0; i < selectedIndices.size(); ++i)
    {
        newPc->m_points.push_back(this->m_points[i]);
        newPc->m_colors.push_back(this->m_colors[i]);
    }

    return newPc;
}