C++ PointCloudOut类代码示例

void 
pcl_ros::VFHEstimation::emptyPublish (const PointCloudInConstPtr &cloud)
{
  PointCloudOut output;
  output.header = cloud->header;
  pub_output_.publish (output.makeShared ());
}

void 
pcl_ros::BoundaryEstimation::computePublish (const PointCloudInConstPtr &cloud, 
                                             const PointCloudNConstPtr &normals,
                                             const PointCloudInConstPtr &surface,
                                             const IndicesPtr &indices)
{
  // Set the parameters
  impl_.setKSearch (k_);
  impl_.setRadiusSearch (search_radius_);
  // Initialize the spatial locator
  initTree (spatial_locator_type_, tree_, k_);
  impl_.setSearchMethod (tree_);

  // Set the inputs
  impl_.setInputCloud (cloud);
  impl_.setIndices (indices);
  impl_.setSearchSurface (surface);
  impl_.setInputNormals (normals);
  // Estimate the feature
  PointCloudOut output;
  impl_.compute (output);

  // Enforce that the TF frame and the timestamp are copied
  output.header = cloud->header;
  pub_output_.publish (output.makeShared ());
}

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> void
pcl::MovingLeastSquares<PointInT, PointOutT>::addProjectedPointNormal (int index,
                                                                       const Eigen::Vector3d &point,
                                                                       const Eigen::Vector3d &normal,
                                                                       double curvature,
                                                                       PointCloudOut &projected_points,
                                                                       NormalCloud &projected_points_normals,
                                                                       PointIndices &corresponding_input_indices) const
{
  PointOutT aux;
  aux.x = static_cast<float> (point[0]);
  aux.y = static_cast<float> (point[1]);
  aux.z = static_cast<float> (point[2]);

  // Copy additional point information if available
  copyMissingFields (input_->points[index], aux);

  projected_points.push_back (aux);
  corresponding_input_indices.indices.push_back (index);

  if (compute_normals_)
  {
    pcl::Normal aux_normal;
    aux_normal.normal_x = static_cast<float> (normal[0]);
    aux_normal.normal_y = static_cast<float> (normal[1]);
    aux_normal.normal_z = static_cast<float> (normal[2]);
    aux_normal.curvature = curvature;
    projected_points_normals.push_back (aux_normal);
  }
}

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

  // 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;

  // For all points
  for (size_t cp = 0; cp < indices_->size (); ++cp)
  {
    // Get the initial estimates of point positions and their neighborhoods
    if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
      continue;


    // Check the number of nearest neighbors for normal estimation (and later
    // for polynomial fit as well)
    if (nn_indices.size () < 3)
      continue;


    PointCloudOut projected_points;
    NormalCloud projected_points_normals;
    // Get a plane approximating the local surface's tangent and project point onto it
    int index = (*indices_)[cp];
    computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]);


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


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

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

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
    pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
    octree.setInputCloud (input_);
    octree.addPointsFromInputCloud ();

    typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
    octree.getOccupiedVoxelCenters (occupied_cells);

    // Determine the voxels crosses along the line segments
    // formed by every pair of occupied cells.
    std::vector< std::vector<int> > line_histograms;
    for (size_t i = 0; i < occupied_cells.size (); ++i)
    {
        Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();

        for (size_t j = i+1; j < occupied_cells.size (); ++j)
        {
            typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
            Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
            octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);

            // Intersected cells are ordered from closest to furthest w.r.t. the origin.
            std::vector<int> histogram;
            for (size_t k = 0; k < intersected_cells.size (); ++k)
            {
                std::vector<int> indices;
                octree.voxelSearch (intersected_cells[k], indices);
                int label = emptyLabel ();
                if (indices.size () != 0)
                {
                    label = getDominantLabel (indices);
                }
                histogram.push_back (label);
            }

            line_histograms.push_back(histogram);
        }
    }

    std::vector< std::vector<int> > transition_histograms;
    computeTransitionHistograms (line_histograms, transition_histograms);

    std::vector<float> distances;
    computeDistancesToMean (transition_histograms, distances);

    std::vector<float> gfpfh_histogram;
    computeDistanceHistogram (distances, gfpfh_histogram);

    output.clear ();
    output.width = 1;
    output.height = 1;
    output.points.resize (1);
    std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}

void 
pcl_ros::NormalEstimation::computePublish (const PointCloudInConstPtr &cloud,
                                           const PointCloudInConstPtr &surface,
                                           const IndicesPtr &indices)
{
  // Set the parameters
  impl_.setKSearch (k_);
  impl_.setRadiusSearch (search_radius_);

  // Set the inputs
  impl_.setInputCloud (cloud);
  impl_.setIndices (indices);
  impl_.setSearchSurface (surface);
  // Estimate the feature
  PointCloudOut output;
  impl_.compute (output);

  // Publish a Boost shared ptr const data
  // Enforce that the TF frame and the timestamp are copied
  output.header = cloud->header;
  pub_output_.publish (output.makeShared ());
}

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, 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> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
  // Compute the number of coefficients
  nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;

  // 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;

  // For all points
  for (size_t cp = 0; cp < indices_->size (); ++cp)
  {
    // Get the initial estimates of point positions and their neighborhoods
    if (!searchForNeighbors (int (cp), nn_indices, nn_sqr_dists))
      continue;

    // Check the number of nearest neighbors for normal estimation (and later
    // for polynomial fit as well)
    if (nn_indices.size () < 3)
      continue;


    PointCloudOut projected_points;
    NormalCloud projected_points_normals;
    // Get a plane approximating the local surface's tangent and project point onto it
    computeMLSPointNormal (int (cp), *input_, nn_indices, nn_sqr_dists, projected_points, projected_points_normals);

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

 
  
  // For the voxel grid upsampling method, generate the voxel grid and dilate it
  // Then, project the newly obtained points to the MLS surface
  if (upsample_method_ == VOXEL_GRID_DILATION)
  {
    MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
    
    for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
      voxel_grid.dilate ();
    
    
    BOOST_FOREACH (typename MLSVoxelGrid::HashMap::value_type voxel, voxel_grid.voxel_grid_)
    {
      // Get 3D position of point
      Eigen::Vector3f pos;
      voxel_grid.getPosition (voxel.first, pos);

      PointInT p;
      p.x = pos[0];
      p.y = pos[1];
      p.z = pos[2];

      std::vector<int> nn_indices;
      std::vector<float> nn_dists;
      tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
      int input_index = nn_indices.front ();

      // If the closest point did not have a valid MLS fitting result
      // OR if it is too far away from the sampled point
      if (mls_results_[input_index].valid == false)
        continue;
      
      Eigen::Vector3f add_point = p.getVector3fMap (),
                      input_point = input_->points[input_index].getVector3fMap ();
      
      Eigen::Vector3d aux = mls_results_[input_index].u;
      Eigen::Vector3f u = aux.cast<float> ();
      aux = mls_results_[input_index].v;
      Eigen::Vector3f v = aux.cast<float> ();
      
      float u_disp = (add_point - input_point).dot (u),
            v_disp = (add_point - input_point).dot (v);
      
      PointOutT result_point;
      pcl::Normal result_normal;
      projectPointToMLSSurface (u_disp, v_disp,
                                mls_results_[input_index].u, mls_results_[input_index].v,
                                mls_results_[input_index].plane_normal,
                                mls_results_[input_index].curvature,
                                input_point,
                                mls_results_[input_index].c_vec,
                                mls_results_[input_index].num_neighbors,
                                result_point, result_normal);
      
      float d_before = (pos - input_point).norm (),
            d_after = (result_point.getVector3fMap () - input_point). norm();
      if (d_after > d_before)
        continue;

      output.push_back (result_point);
      if (compute_normals_)
        normals_->push_back (result_normal);
    }
  }

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
                                                                     const PointCloudIn &input,
                                                                     const std::vector<int> &nn_indices,
                                                                     std::vector<float> &nn_sqr_dists,
                                                                     PointCloudOut &projected_points,
                                                                     NormalCloud &projected_points_normals)
{
  // Compute the plane coefficients
  //pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
  Eigen::Vector4f xyz_centroid;

  // Estimate the XYZ centroid
  pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
  //pcl::compute3DCentroid (input, nn_indices, xyz_centroid);

  pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
  // Compute the 3x3 covariance matrix

  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  Eigen::Vector4f model_coefficients;
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
  model_coefficients.head<3> () = eigen_vector;
  model_coefficients[3] = 0;
  model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);

  // Projected query point
  Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
  float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
  point -= distance * model_coefficients.head<3> ();

  float curvature = covariance_matrix.trace ();
  // Compute the curvature surface change
  if (curvature != 0)
    curvature = fabsf (eigen_value / curvature);


  // Get a copy of the plane normal easy access
  Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
  // Vector in which the polynomial coefficients will be put
  Eigen::VectorXd c_vec;
  // Local coordinate system (Darboux frame)
  Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);



  // Perform polynomial fit to update point and normal
  ////////////////////////////////////////////////////
  if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
  {
    // Update neighborhood, since point was projected, and computing relative
    // positions. Note updating only distances for the weights for speed
    std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
    for (size_t ni = 0; ni < nn_indices.size (); ++ni)
    {
      de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
      de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
      de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
      nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
    }

    // Allocate matrices and vectors to hold the data used for the polynomial fit
    Eigen::VectorXd weight_vec (nn_indices.size ());
    Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
    Eigen::VectorXd f_vec (nn_indices.size ());
    Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
    Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);

    // Get local coordinate system (Darboux frame)
    v = plane_normal.unitOrthogonal ();
    u = plane_normal.cross (v);

    // Go through neighbors, transform them in the local coordinate system,
    // save height and the evaluation of the polynome's terms
    double u_coord, v_coord, u_pow, v_pow;
    for (size_t ni = 0; ni < nn_indices.size (); ++ni)
    {
      // (re-)compute weights
      weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);

      // transforming coordinates
      u_coord = de_meaned[ni].dot (u);
      v_coord = de_meaned[ni].dot (v);
      f_vec (ni) = de_meaned[ni].dot (plane_normal);

      // compute the polynomial's terms at the current point
      int j = 0;
      u_pow = 1;
      for (int ui = 0; ui <= order_; ++ui)
      {
        v_pow = 1;
        for (int vi = 0; vi <= order_ - ui; ++vi)
        {
          P (j++, ni) = u_pow * v_pow;
          v_pow *= v_coord;
        }
        u_pow *= u_coord;
      }
//.........这里部分代码省略.........

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));
//.........这里部分代码省略.........

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::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;
}

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output)
{
  if (upsample_method_ == DISTINCT_CLOUD)
  {
    for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
    {
      // Distinct cloud may have nan points, skip them
      if (!pcl_isfinite (distinct_cloud_->points[dp_i].x))
        continue;

      // Get 3D position of point
      //Eigen::Vector3f pos = distinct_cloud_->points[dp_i].getVector3fMap ();
      std::vector<int> nn_indices;
      std::vector<float> nn_dists;
      tree_->nearestKSearch (distinct_cloud_->points[dp_i], 1, nn_indices, nn_dists);
      int input_index = nn_indices.front ();

      // If the closest point did not have a valid MLS fitting result
      // OR if it is too far away from the sampled point
      if (mls_results_[input_index].valid == false)
        continue;

      Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();

      float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
            v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));

      PointOutT result_point;
      pcl::Normal result_normal;
      projectPointToMLSSurface (u_disp, v_disp,
                                mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
                                mls_results_[input_index].plane_normal,
                                mls_results_[input_index].mean,
                                mls_results_[input_index].curvature,
                                mls_results_[input_index].c_vec,
                                mls_results_[input_index].num_neighbors,
                                result_point, result_normal);

      // Copy additional point information if available
      copyMissingFields (input_->points[input_index], result_point);

      // Store the id of the original point
      corresponding_input_indices_->indices.push_back (input_index);

      output.push_back (result_point);
      if (compute_normals_)
        normals_->push_back (result_normal);
    }
  }

  // For the voxel grid upsampling method, generate the voxel grid and dilate it
  // Then, project the newly obtained points to the MLS surface
  if (upsample_method_ == VOXEL_GRID_DILATION)
  {
    MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
    for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
      voxel_grid.dilate ();

    for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it)
    {
      // Get 3D position of point
      Eigen::Vector3f pos;
      voxel_grid.getPosition (m_it->first, pos);

      PointInT p;
      p.x = pos[0];
      p.y = pos[1];
      p.z = pos[2];

      std::vector<int> nn_indices;
      std::vector<float> nn_dists;
      tree_->nearestKSearch (p, 1, nn_indices, nn_dists);
      int input_index = nn_indices.front ();

      // If the closest point did not have a valid MLS fitting result
      // OR if it is too far away from the sampled point
      if (mls_results_[input_index].valid == false)
        continue;

      Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
      float u_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].u_axis)),
            v_disp = static_cast<float> ((add_point - mls_results_[input_index].mean).dot (mls_results_[input_index].v_axis));

      PointOutT result_point;
      pcl::Normal result_normal;
      projectPointToMLSSurface (u_disp, v_disp,
                                mls_results_[input_index].u_axis, mls_results_[input_index].v_axis,
                                mls_results_[input_index].plane_normal,
                                mls_results_[input_index].mean,
                                mls_results_[input_index].curvature,
                                mls_results_[input_index].c_vec,
                                mls_results_[input_index].num_neighbors,
                                result_point, result_normal);

      // Copy additional point information if available
      copyMissingFields (input_->points[input_index], result_point);

      // Store the id of the original point
      corresponding_input_indices_->indices.push_back (input_index);
//.........这里部分代码省略.........