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

• • 前言
  • 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 , x y , y 2 1,x,y,x^2,xy,y^2 1,x,y,x2,xy,y2等6項， 6 = ( 2 + 1 ) ∗ ( 2 + 2 ) / 2 6 = (2+1)*(2+2)/2 6=(2+1)∗(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);
}