C++ PointCloudOut::size方法代码示例

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl16::PPFRGBRegionEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  PCL16_INFO ("before computing output size: %u\n", output.size ());
  output.resize (indices_->size ());
  for (int index_i = 0; index_i < static_cast<int> (indices_->size ()); ++index_i)
  {
    int i = (*indices_)[index_i];
    std::vector<int> nn_indices;
    std::vector<float> nn_distances;
    tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);

    PointOutT average_feature_nn;
    average_feature_nn.alpha_m = 0;
    average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
        average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;

    for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
    {
      int j = *nn_it;
      if (i != j)
      {
        float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
        if (pcl16::computeRGBPairFeatures
            (input_->points[i].getVector4fMap (), normals_->points[i].getNormalVector4fMap (), input_->points[i].getRGBVector4i (),
             input_->points[j].getVector4fMap (), normals_->points[j].getNormalVector4fMap (), input_->points[j].getRGBVector4i (),
             f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
        {
          average_feature_nn.f1 += f1;
          average_feature_nn.f2 += f2;
          average_feature_nn.f3 += f3;
          average_feature_nn.f4 += f4;
          average_feature_nn.r_ratio += r_ratio;
          average_feature_nn.g_ratio += g_ratio;
          average_feature_nn.b_ratio += b_ratio;
        }
        else
        {
          PCL16_ERROR ("[pcl16::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j);
        }
      }
    }

    float normalization_factor = static_cast<float> (nn_indices.size ());
    average_feature_nn.f1 /= normalization_factor;
    average_feature_nn.f2 /= normalization_factor;
    average_feature_nn.f3 /= normalization_factor;
    average_feature_nn.f4 /= normalization_factor;
    average_feature_nn.r_ratio /= normalization_factor;
    average_feature_nn.g_ratio /= normalization_factor;
    average_feature_nn.b_ratio /= normalization_factor;
    output.points[index_i] = average_feature_nn;
  }
  PCL16_INFO ("Output size: %u\n", output.points.size ());
}

template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
  // image size
  const int width = int (input_->width);
  const int height = int (input_->height);

  // destination for intensity data; will be forwarded to BRISK
  std::vector<unsigned char> image_data (width*height);

  for (size_t i = 0; i < image_data.size (); ++i)
    image_data[i] = static_cast<unsigned char> (intensity_ ((*input_)[i]));

  pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_);
  brisk_scale_space.constructPyramid (image_data, width, height);
  // Check if the template types are the same. If true, avoid a copy.
  // The PointOutT MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointOutT, pcl::PointWithScale> ())
    brisk_scale_space.getKeypoints (threshold_, output.points);
  else
  {
    pcl::PointCloud<pcl::PointWithScale> output_temp;
    brisk_scale_space.getKeypoints (threshold_, output_temp.points);
    pcl::copyPointCloud<pcl::PointWithScale, PointOutT> (output_temp, output);
  }

  // we do not change the denseness
  output.width = int (output.points.size ());
  output.height = 1;
  output.is_dense = false;      // set to false to be sure

  // 2nd pass to remove the invalid set of 3D keypoints
  if (remove_invalid_3D_keypoints_)
  {
    PointCloudOut output_clean;
    for (size_t i = 0; i < output.size (); ++i)
    {
      PointOutT pt;
      // Interpolate its position in 3D, as the "u" and "v" are subpixel accurate
      bilinearInterpolation (input_, output[i].x, output[i].y, pt);

      // Check if the point is finite
      if (pcl::isFinite (pt))
        output_clean.push_back (output[i]);
    }
    output = output_clean;
    output.is_dense = true;      // set to true as there's no keypoint at an invalid XYZ
  }
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
  response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
  const Normals &normals = *normals_;
  const PointCloudIn &input = *input_;
  pcl::PointCloud<float>& response = *response_;
  const int w = static_cast<int> (input_->width) - half_window_size_;
  const int h = static_cast<int> (input_->height) - half_window_size_;

  if (method_ == FOUR_CORNERS)
  {
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
    for(int j = half_window_size_; j < h; ++j)
    {
      for(int i = half_window_size_; i < w; ++i)
      {
        if (!isFinite (input (i,j))) continue;
        const NormalT &center = normals (i,j);
        if (!isFinite (center)) continue;

        int count = 0;
        const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
        const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
        const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
        const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
        // Get rid of isolated points
        if (!count) continue;

        float sn1 = squaredNormalsDiff (up, center);
        float sn2 = squaredNormalsDiff (down, center);
        float r1 = sn1 + sn2;
        float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center);

        float d = std::min (r1, r2);
        if (d < first_threshold_) continue;

        sn1 = sqrt (sn1);
        sn2 = sqrt (sn2);
        float b1 = normalsDiff (right, up) * sn1;
        b1+= normalsDiff (left, down) * sn2;
        float b2 = normalsDiff (right, down) * sn2;
        b2+= normalsDiff (left, up) * sn1;
        float B = std::min (b1, b2);
        float A = r2 - r1 - 2*B;

        response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
      }
    }
  }
  else
  {
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
    for(int j = half_window_size_; j < h; ++j)
    {
      for(int i = half_window_size_; i < w; ++i)
      {
        if (!isFinite (input (i,j))) continue;
        const NormalT &center = normals (i,j);
        if (!isFinite (center)) continue;

        int count = 0;
        const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
        const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
        const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
        const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
        const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
        const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
        const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
        const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
        // Get rid of isolated points
        if (!count) continue;

        std::vector<float> r (4,0);

        r[0] = squaredNormalsDiff (up, center);
        r[0]+= squaredNormalsDiff (down, center);

        r[1] = squaredNormalsDiff (upright, center);
        r[1]+= squaredNormalsDiff (downleft, center);

        r[2] = squaredNormalsDiff (right, center);
        r[2]+= squaredNormalsDiff (left, center);

        r[3] = squaredNormalsDiff (downright, center);
        r[3]+= squaredNormalsDiff (upleft, center);

        float d = *(std::min_element (r.begin (), r.end ()));

        if (d < first_threshold_) continue;

        std::vector<float> B (4,0);
        std::vector<float> A (4,0);
        std::vector<float> sumAB (4,0);
        B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
        B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
//.........这里部分代码省略.........

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
  // Check if normals have to be computed/saved
  if (compute_normals_)
  {
    normals_.reset (new NormalCloud);
    // Copy the header
    normals_->header = input_->header;
    // Clear the fields in case the method exits before computation
    normals_->width = normals_->height = 0;
    normals_->points.clear ();
  }


  // Copy the header
  output.header = input_->header;
  output.width = output.height = 0;
  output.points.clear ();

  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
  {
    PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
    return;
  }

  if (!initCompute ())
    return;


  // Initialize the spatial locator
  if (!tree_)
  {
    KdTreePtr tree;
    if (input_->isOrganized ())
      tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
    else
      tree.reset (new pcl::search::KdTree<PointInT> (false));
    setSearchMethod (tree);
  }

  // Send the surface dataset to the spatial locator
  tree_->setInputCloud (input_, indices_);

  // Initialize random number generator if necessary
  switch (upsample_method_)
  {
    case (RANDOM_UNIFORM_DENSITY):
    {
      boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0)));
      float tmp = static_cast<float> (search_radius_ / 2.0f);
      boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp);
      rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib);
      break;
    }
    case (VOXEL_GRID_DILATION):
    {
      mls_results_.resize (input_->size ());
      break;
    }
    default:
      break;
  }

  // Perform the actual surface reconstruction
  performProcessing (output);

  if (compute_normals_)
  {
    normals_->height = 1;
    normals_->width = static_cast<uint32_t> (normals_->size ());

    // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point
    for (unsigned int i = 0; i < output.size (); ++i)
    {
      typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature));
    }

  }

  // Set proper widths and heights for the clouds
  output.height = 1;
  output.width = static_cast<uint32_t> (output.size ());

  deinitCompute ();
}

template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
  derivatives_cols_.resize (input_->width, input_->height);
  derivatives_rows_.resize (input_->width, input_->height);
  //Compute cloud intensities first derivatives along columns and rows
  //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan
  int w = static_cast<int> (input_->width) - 1;
  int h = static_cast<int> (input_->height) - 1;
  // j = 0 --> j-1 out of range ; use 0
  // i = 0 --> i-1 out of range ; use 0
  derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
  derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;

// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
  for(int i = 1; i < w; ++i)
	{
		derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
	}

  derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
  derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;

// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
  for(int j = 1; j < h; ++j)
  {
    // i = 0 --> i-1 out of range ; use 0
		derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
    for(int i = 1; i < w; ++i)
    {
      // derivative with respect to rows
      derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;

      // derivative with respect to cols
      derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
    }
    // i = w --> w+1 out of range ; use w
    derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
  }

  // j = h --> j+1 out of range use h
  derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
  derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;

// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
  for(int i = 1; i < w; ++i)
	{
    derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
	}
  derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
  derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;

  float highest_response_;
  
  switch (method_)
  {
    case HARRIS:
      responseHarris(*response_, highest_response_);
      break;
    case NOBLE:
      responseNoble(*response_, highest_response_);
      break;
    case LOWE:
      responseLowe(*response_, highest_response_);
      break;
    case TOMASI:
      responseTomasi(*response_, highest_response_);
      break;
  }
  
  if (!nonmax_)
    output = *response_;
  else
  {    
    threshold_*= highest_response_;

    std::sort (indices_->begin (), indices_->end (), 
               boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
    
    output.clear ();
    output.reserve (response_->size());
    std::vector<bool> occupency_map (response_->size (), false);    
    int width (response_->width);
    int height (response_->height);
    const int occupency_map_size (occupency_map.size ());

#ifdef _OPENMP
#pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)   
#endif
    for (int idx = 0; idx < occupency_map_size; ++idx)
    {
//.........这里部分代码省略.........

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
  // Reset or initialize the collection of indices
  corresponding_input_indices_.reset (new PointIndices);

  // Check if normals have to be computed/saved
  if (compute_normals_)
  {
    normals_.reset (new NormalCloud);
    // Copy the header
    normals_->header = input_->header;
    // Clear the fields in case the method exits before computation
    normals_->width = normals_->height = 0;
    normals_->points.clear ();
  }

  // Copy the header
  output.header = input_->header;
  output.width = output.height = 0;
  output.points.clear ();

  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
  {
    PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
    return;
  }

  // Check if distinct_cloud_ was set
  if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_)
  {
    PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ());
    return;
  }

  if (!initCompute ())
    return;

  // Initialize the spatial locator
  if (!tree_)
  {
    KdTreePtr tree;
    if (input_->isOrganized ())
      tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
    else
      tree.reset (new pcl::search::KdTree<PointInT> (false));
    setSearchMethod (tree);
  }

  // Send the surface dataset to the spatial locator
  tree_->setInputCloud (input_);

  switch (upsample_method_)
  {
    // Initialize random number generator if necessary
    case (RANDOM_UNIFORM_DENSITY):
    {
      rng_alg_.seed (static_cast<unsigned> (std::time (0)));
      float tmp = static_cast<float> (search_radius_ / 2.0f);
      boost::uniform_real<float> uniform_distrib (-tmp, tmp);
      rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));

      break;
    }
    case (VOXEL_GRID_DILATION):
    case (DISTINCT_CLOUD):
    {
      if (!cache_mls_results_)
        PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");

      cache_mls_results_ = true;
      break;
    }
    default:
      break;
  }

  if (cache_mls_results_)
  {
    mls_results_.resize (input_->size ());
  }
  else
  {
    mls_results_.resize (1); // Need to have a reference to a single dummy result.
  }

  // Perform the actual surface reconstruction
  performProcessing (output);

  if (compute_normals_)
  {
    normals_->height = 1;
    normals_->width = static_cast<uint32_t> (normals_->size ());

    for (unsigned int i = 0; i < output.size (); ++i)
    {
      typedef typename pcl::traits::fieldList<PointOutT>::type FieldList;
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y));
      pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z));
//.........这里部分代码省略.........