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

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 ());
}

void
pcl::recognition::ORROctree::getFullLeavesPoints (PointCloudOut& out) const
{
  out.resize(full_leaves_.size ());
  size_t i = 0;

  // Now iterate over all full leaves and compute the normals and average points
  for ( vector<ORROctree::Node*>::const_iterator it = full_leaves_.begin() ; it != full_leaves_.end() ; ++it, ++i )
  {
    out[i].x = (*it)->getData ()->getPoint ()[0];
    out[i].y = (*it)->getData ()->getPoint ()[1];
    out[i].z = (*it)->getData ()->getPoint ()[2];
  }
}

template <typename PointInT, typename PointOutT> void
pcl::BilateralUpsampling<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
    output.resize (input_->size ());
    float nan = std::numeric_limits<float>::quiet_NaN ();

    Eigen::MatrixXf val_exp_depth_matrix;
    Eigen::VectorXf val_exp_rgb_vector;
    computeDistances (val_exp_depth_matrix, val_exp_rgb_vector);

    for (int x = 0; x < static_cast<int> (input_->width); ++x)
      for (int y = 0; y < static_cast<int> (input_->height); ++y)
      {
        int start_window_x = std::max (x - window_size_, 0),
            start_window_y = std::max (y - window_size_, 0),
            end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)),
            end_window_y = std::min (y + window_size_, static_cast<int> (input_->height));

        float sum = 0.0f,
            norm_sum = 0.0f;

        for (int x_w = start_window_x; x_w < end_window_x; ++ x_w)
          for (int y_w = start_window_y; y_w < end_window_y; ++ y_w)
          {
            float dx = float (x - x_w),
                dy = float (y - y_w);

            float val_exp_depth = val_exp_depth_matrix(dx+window_size_, dy+window_size_);

            float d_color = static_cast<float> (
                abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) +
                abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) +
                abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b));
            
            float val_exp_rgb = val_exp_rgb_vector(Eigen::VectorXf::Index(d_color));

            if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z))
            {
              sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z;
              norm_sum += val_exp_depth * val_exp_rgb;
            }
          }

        output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r;
        output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g;
        output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b;

        if (norm_sum != 0.0f)
        {
          float depth = sum / norm_sum;
          Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth);
          Eigen::Vector3f pw (unprojection_matrix_ * pc);
          output.points[y*input_->width + x].x = pw[0];
          output.points[y*input_->width + x].y = pw[1];
          output.points[y*input_->width + x].z = pw[2];
        }
        else
        {
          output.points[y*input_->width + x].x = nan;
          output.points[y*input_->width + x].y = nan;
          output.points[y*input_->width + x].z = nan;
        }
      }

    output.header = input_->header;
    output.width = input_->width;
    output.height = input_->height;
}