openMP---第一篇

openMP 处理for循环

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
  // Compute the number of coefficients
  nr_coeff_ = (order_ + ) * (order_ + ) / ;

  size_t mls_result_index = ;

#ifdef _OPENMP
  // (Maximum) number of threads
  const unsigned int threads = threads_ ==  ?  : 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
#ifdef _OPENMP
#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
#endif
  for (int cp = ; 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 ().
    std::vector<int> nn_indices;
    std::vector<float> nn_sqr_dists;

    // Get the initial estimates of point positions and their neighborhoods
    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 () >= )
      {
        // 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
        size_t pp_size = projected_points[tn].size ();
#else
        PointCloudOut projected_points;
        NormalCloud projected_points_normals;
#endif

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

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

#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 (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
          copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
#else
        computeMLSPointNormal (index, nn_indices, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);

        // Append projected points to output
        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
      }
    }
  }

#ifdef _OPENMP
  // Combine all threads' results into the output vectors
  for (unsigned int tn = ; 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

  // Perform the distinct-cloud or voxel-grid upsampling
  performUpsampling (output);
}

template <typename PointT> void
pcl::FastBilateralFilterOMP<PointT>::applyFilter (PointCloud &output)
{
  if (!input_->isOrganized ())
  {
    PCL_ERROR ("[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n");
    return;
  }

  copyPointCloud (*input_, output);
  float base_max = -std::numeric_limits<float>::max (),
        base_min = std::numeric_limits<float>::max ();
  bool found_finite = false;
  for (size_t x = ; x < output.width; ++x)
  {
    for (size_t y = ; y < output.height; ++y)
    {
      if (pcl_isfinite (output (x, y).z))
      {
        if (base_max < output (x, y).z)
          base_max = output (x, y).z;
        if (base_min > output (x, y).z)
          base_min = output (x, y).z;
        found_finite = true;
      }
    }
  }
  if (!found_finite)
  {
    PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n");
    return;
  }
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
  for (long int i = ; i < static_cast<long int> (output.size ()); ++i)
    if (!pcl_isfinite (output.at(i).z))
      output.at(i).z = base_max;

  const float base_delta = base_max - base_min;

  const size_t padding_xy = ;
  const size_t padding_z  = ;

  const size_t small_width  = static_cast<size_t> (static_cast<float> (input_->width  - ) / sigma_s_) +  +  * padding_xy;
  const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - ) / sigma_s_) +  +  * padding_xy;
  const size_t small_depth  = static_cast<size_t> (base_delta / sigma_r_)   +  +  * padding_z;

  Array3D data (small_width, small_height, small_depth);
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
  for (long int i = ; i < static_cast<long int> (small_width * small_height); ++i)
  {
    size_t small_x = static_cast<size_t> (i % small_width);
    size_t small_y = static_cast<size_t> (i / small_width);
    size_t start_x = static_cast<size_t>(
        std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + , .f));
    size_t end_x = static_cast<size_t>(
      std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + , .f));
    size_t start_y = static_cast<size_t>(
      std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + , .f));
    size_t end_y = static_cast<size_t>(
      std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + , .f));
    for (size_t x = start_x; x < end_x && x < input_->width; ++x)
    {
      for (size_t y = start_y; y < end_y && y < input_->height; ++y)
      {
        const float z = output (x,y).z - base_min;
        const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
        Eigen::Vector2f& d = data (small_x, small_y, small_z);
        d[] += output (x,y).z;
        d[] += 1.0f;
      }
    }
  }

  std::vector<long int> offset ();
  offset[] = &(data (,,)) - &(data (,,));
  offset[] = &(data (,,)) - &(data (,,));
  offset[] = &(data (,,)) - &(data (,,));

  Array3D buffer (small_width, small_height, small_depth);

  for (size_t dim = ; dim < ; ++dim)
  {
    for (size_t n_iter = ; n_iter < ; ++n_iter)
    {
      Array3D* current_buffer = (n_iter %  ==  ? &buffer : &data);
      Array3D* current_data =(n_iter %  ==  ? &data : &buffer);
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
      for(long int i = ; i < static_cast<long int> ((small_width - )*(small_height - )); ++i)
      {
        size_t x = static_cast<size_t> (i % (small_width - ) + );
        size_t y = static_cast<size_t> (i / (small_width - ) + );
        const long int off = offset[dim];
        Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,));
        Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,));

        for(size_t z = ; z < small_depth - ; ++z, ++d_ptr, ++b_ptr)
          *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
      }
    }
  }
  // Note: this works because there are an even number of iterations.
  // If there were an odd number, we would need to end with a:
  // std::swap (data, buffer);

  if (early_division_)
  {
    for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
      *d /= ((*d)[] != ) ? (*d)[] : ;

#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
    for (long int i = ; i < static_cast<long int> (input_->size ()); ++i)
    {
      size_t x = static_cast<size_t> (i % input_->width);
      size_t y = static_cast<size_t> (i / input_->width);
      const float z = output (x,y).z - base_min;
      const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
                                                              static_cast<float> (y) / sigma_s_ + padding_xy,
                                                              z / sigma_r_ + padding_z);
      output(x,y).z = D[];
    }
  }
  else
  {
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
    for (long i = ; i < static_cast<long int> (input_->size ()); ++i)
    {
      size_t x = static_cast<size_t> (i % input_->width);
      size_t y = static_cast<size_t> (i / input_->width);
      const float z = output (x,y).z - base_min;
      const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
                                                              static_cast<float> (y) / sigma_s_ + padding_xy,
                                                              z / sigma_r_ + padding_z);
      output (x,y).z = D[] / D[];
    }
  }
}

template <typename PointInT, typename PointOutT> void
pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;
  // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
  if (input_->is_dense)
  {
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
    // Iterating over the entire index vector
    for (int idx = ; idx < static_cast<int> (indices_->size ()); ++idx)
    {
      Eigen::Vector4f n;
      if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) ==  ||
          !computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature))
      {
        output.points[idx].normal[] = output.points[idx].normal[] = output.points[idx].normal[] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

        output.is_dense = false;
        continue;
      }

      output.points[idx].normal_x = n[];
      output.points[idx].normal_y = n[];
      output.points[idx].normal_z = n[];

      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[], output.points[idx].normal[], output.points[idx].normal[]);

    }
  }
  else
  {
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
    // Iterating over the entire index vector
    for (int idx = ; idx < static_cast<int> (indices_->size ()); ++idx)
    {
      Eigen::Vector4f n;
      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) ==  ||
          !computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature))
      {
        output.points[idx].normal[] = output.points[idx].normal[] = output.points[idx].normal[] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

        output.is_dense = false;
        continue;
      }

      output.points[idx].normal_x = n[];
      output.points[idx].normal_y = n[];
      output.points[idx].normal_z = n[];

      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[], output.points[idx].normal[], output.points[idx].normal[]);

    }
  }
}