PCL中的FPS(最远点采样)源码

原理：

1.首先，对于三维点云数据，我们一般采取欧式距离度量点之间距离，即空间中两点直线距离。

  2.在对第一个点采样时，理论上我们可以随机从数据中选取一个点。另一种规范的做法是：求整个数据点(点云)的重心(即所有点坐标求和平均得到的坐标点)，选取距离重心最远的点，记为P0。

  3. 然后，我们继续选取剩余的所有点中距离P0最远的点，记为P1。

  4. 对于剩下的每个点，分别计算到P0和P1的距离，并选取最短的那个作为这个点到P0,P1整体的距离。计算这些距离后选择距离最大的那个点，记为P2。

  5. 依次重复操作，直到选取所需数目的点。例如：继续选取点，分别计算剩余各点到P0,P1,P2的距离，并选取最短的那个距离作为某点到P1，P2，P3整体的距离，然后选取这些点中距离最大的那个点，记为P3。
原文链接：https://blog.csdn.net/weixin_60737527/article/details/127432708

template<typename PointT> void
 pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
 {
	//获取输入点云的大小（点的数量）。
   const std::size_t size = input_->size();
   //if requested number of point is equal to the point cloud size, copy original cloud
   //如果请求的采样点数与点云的点数相等，则直接将原始索引赋值给输出索引并清空 removed_indices_ （如果存在），然后函数返回。
   if (sample_size_ == size)
   {
     indices = *indices_;
     removed_indices_->clear ();
     return;
   }
   //如果请求的采样点数大于点云的点数，则抛出异常 BadArgumentException。
   //check if requested number of points is greater than the point cloud size
   if (sample_size_ > size)
   {
     PCL_THROW_EXCEPTION (BadArgumentException,
                         "Requested number of points is greater than point cloud size!");
   }
   //创建一个大小为 size 的浮点数向量 distances_to_selected_points，并初始化所有元素的值为 float 类型的最大值。
   std::vector<float> distances_to_selected_points (size, std::numeric_limits<float>::max ());
   
   //set random seed
   //初始化伪随机数生成器 random_gen，并且将生成的随机数限定在 [0, size-1] 的范围内。
   std::mt19937 random_gen(seed_);
   std::uniform_int_distribution<index_t> dis(0, size -1);
  
   //pick the first point at random
   //用随机生成的索引作为起点的索引，将其距离设为 -1.0，并将该索引添加到输出索引 indices 中。
   index_t max_index = dis(random_gen);
   distances_to_selected_points[max_index] = -1.0;
   indices.push_back(max_index);
   //循环从第二个点开始，直到获取到所需采样点数。
   for (std::size_t j = 1; j < sample_size_; ++j)
   {
   //初始化下一个最远点的索引 next_max_index，并获取当前最远点的坐标 max_index_point。
     index_t next_max_index = 0;
     
     const PointT& max_index_point = (*input_)[max_index];
     //recompute distances
	 //计算未选择点到当前最远点的距离，并更新下一个最远点的索引 next_max_index。
     for (std::size_t i = 0; i < size; ++i)
     {
       if (distances_to_selected_points[i] == -1.0)
         continue;
       distances_to_selected_points[i] = std::min(distances_to_selected_points[i], geometry::distance((*input_)[i], max_index_point));
       if (distances_to_selected_points[i] > distances_to_selected_points[next_max_index])
         next_max_index = i;
     }
  
     //select farthest point based on previously calculated distances
     //since distance is set to -1 for all selected elements,previously selected 
     //elements are guaranteed to not be selected
	 //更新最远点的索引为下一个最远点的索引，将该索引添加到输出索引 indices 中，并将其距离设为 -1.0，以确保下一次迭代中不会再选择该点。
     max_index = next_max_index;
     distances_to_selected_points[max_index] = -1.0;
     indices.push_back(max_index);
     //set distance to -1 to ignore during max element search
   }
  //如果提取已移除的点的索引（extract_removed_indices_ 为真），则将未被选择的点的索引添加到 removed_indices_（如果存在） 中。
   if (extract_removed_indices_)
   {
     for (std::size_t k = 0; k < distances_to_selected_points.size(); ++k)
     {
       if (distances_to_selected_points[k] != -1.0)
         (*removed_indices_).push_back(k);
     }
   }
 }