PCL - MLS代碼研讀（十）- performProcessing函數

前言

在PCL - MLS代碼研讀（九）- MLS入口 - process函數中，看到了process函數實際上是performProcessing函數的wrapper，真正做事的其實是performProcessing函數。

performProcessing

對於輸入點雲中的所有點，都調用一次computeMLSPointNormal函數，計算得到投影點坐標projected_points，投影點法向量projected_points_normals，並更新corresponding_input_indices。最後再把計算結果新增到output和normals_裡面。

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{

計算多項式係數的數量：

  // Compute the number of coefficients
  /*
  1
  x, y
  x^2, x*y, y^2
  x^3, x^2*y, x*y^2, y^3

  it's a triangle with upper length = 1 and bottom length = n+1
  */
  nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;

例：二次多項式有$1,x,y,x^2,xy,y^2$等6項，$6=(2+1)\ast (2+2)/2$。

開啟OPENMP才會運行，先略過。

#ifdef _OPENMP
  // (Maximum) number of threads
  const unsigned int threads = threads_ == 0 ? 1 : threads_;
  // Create temporaries for each thread in order to avoid synchronization
  typename PointCloudOut::CloudVectorType projected_points (threads);
  typename NormalCloud::CloudVectorType projected_points_normals (threads);
  std::vector<PointIndices> corresponding_input_indices (threads);
#endif

對於輸入點雲中的所有點，都做以下事情：

  // For all points
#pragma omp parallel for \
  default(none) \
  shared(corresponding_input_indices, projected_points, projected_points_normals) \
  schedule(dynamic,1000) \
  num_threads(threads)
  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
  {

搜索最近鄰：

    // Allocate enough space to hold the results of nearest neighbor searches
    // \note resize is irrelevant for a radiusSearch ().
    pcl::Indices nn_indices;
    std::vector<float> nn_sqr_dists;

    // Get the initial estimates of point positions and their neighborhoods
    // cp: indices_的索引,(*indices_)[cp]:真正的索引
    if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
    {

檢查鄰居數量：

      // Check the number of nearest neighbors for normal estimation (and later for polynomial fit as well)
      if (nn_indices.size () >= 3)
      {

宣告用於儲存投影點本身及法向量的數據結構：

        // This thread's ID (range 0 to threads-1)
#ifdef _OPENMP
        const int tn = omp_get_thread_num ();
        // Size of projected points before computeMLSPointNormal () adds points
        std::size_t pp_size = projected_points[tn].size ();
#else
        // 對於(*indices_)[cp]這個點的投影結果
        PointCloudOut projected_points;
        NormalCloud projected_points_normals;
#endif

indices_是儲存要處理的點的索引的數據結構，注意(*indices_)[cp]才是真正的索引：

        // Get a plane approximating the local surface's tangent and project point onto it
        const int index = (*indices_)[cp];

快取MLS的結果：

        std::size_t mls_result_index = 0;
        if (cache_mls_results_)
          mls_result_index = index; // otherwise we give it a dummy location.

開啟OPENMP才會運行，先略過。

#ifdef _OPENMP
        computeMLSPointNormal (index, nn_indices, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], mls_results_[mls_result_index]);

        // Copy all information from the input cloud to the output points (not doing any interpolation)
        for (std::size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
          copyMissingFields ((*input_)[(*indices_)[cp]], projected_points[tn][pp]);

調用computeMLSPointNormal函數，計算得到投影點坐標projected_points，投影點法向量projected_points_normals，並更新corresponding_input_indices。

#else
        // 對input_中的每個點,都對它周遭的點做投影,得到projected_points?
        // 依據入參index,nn_indices來更新projected_points,projected_points_normals,corresponding_input_indices
        computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);

最後再把計算結果新增到output和normals_裡面。

        // Append projected points to output
        /**
         * 如果upsample_method_是NONE,
         * 那麼input_中的每一個點只會產生一個投影點
         * 如果是SAMPLE_LOCAL_PLANE或RANDOM_UNIFORM_DENSITY,
         * 那麼input_中的每一個點只會產生多個投影點
         * 這裡再把生成的投影點加入output
         * 如果是DISTINCT_CLOUD或VOXEL_GRID_DILATION,
         * 則不會在這裡做上採樣,而是等到投影結束之後再做performUpsampling
         **/
        output.insert (output.end (), projected_points.begin (), projected_points.end ());
        if (compute_normals_)
          normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
#endif
      }
    }
  }

開啟OPENMP才會運行，先略過。

#ifdef _OPENMP
  // Combine all threads' results into the output vectors
  for (unsigned int tn = 0; tn < threads; ++tn)
  {
    output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
    corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
                                                  corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
    if (compute_normals_)
      normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
  }
#endif

如果上採樣方式是DISTINCT_CLOUD或VOXEL_GRID_DILATION，則會調用performUpsampling函數做上採樣。

  // Perform the distinct-cloud or voxel-grid upsampling
  /**
   * 如果upsample_method_是DISTINCT_CLOUD或VOXEL_GRID_DILATION,
   * 會到投影結束之後再做performUpsampling
   **/
  performUpsampling (output);
}