예제 #1
0
파일: fpfh.hpp 프로젝트: jonaswitt/nestk
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Check if input was set
  if (!normals_)
  {
    ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  if (normals_->points.size () != surface_->points.size ())
  {
    ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

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

  size_t data_size = indices_->size ();
  // Reset the whole thing
  hist_f1_.setZero (data_size, nr_bins_f1_);
  hist_f2_.setZero (data_size, nr_bins_f2_);
  hist_f3_.setZero (data_size, nr_bins_f3_);

  int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;

  // Iterating over the entire index vector
  for (size_t idx = 0; idx < data_size; ++idx)
  {
    int p_idx = (*indices_)[idx];
    searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);

    // Estimate the FPFH signature at each patch
    computePointSPFHSignature (*surface_, *normals_, p_idx, nn_indices,
                               hist_f1_, hist_f2_, hist_f3_);
  }

  fpfh_histogram_.setZero (nr_bins);
  // Iterating over the entire index vector
  for (size_t idx = 0; idx < data_size; ++idx)
  {
    searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);

    weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);

    // Copy into the resultant cloud
    for (int d = 0; d < fpfh_histogram_.size (); ++d)
      output.points[idx].histogram[d] = fpfh_histogram_[d];
    fpfh_histogram_.setZero ();
  }

}
예제 #2
0
template <typename PointInT, typename PointOutT> void
pcl::NormalEstimation<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_);

  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    if (!searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
    {
      output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
      continue;
    }

    computePointNormal (*surface_, nn_indices,
                        output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);

    flipNormalTowardsViewpoint (surface_->points[idx], vpx_, vpy_, vpz_,
                                output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);

  }
}
예제 #3
0
파일: mls.hpp 프로젝트: Cakem1x/pcl
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);
}
예제 #4
0
template <typename PointInT, typename PointOutT> void
pcl::MomentInvariantsEstimation<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_);

  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);

    computePointMomentInvariants (*surface_, nn_indices,
                                  output.points[idx].j1, output.points[idx].j2, output.points[idx].j3);
  }
}
예제 #5
0
template <typename PointInT, typename NormalOutT> void
pcl::MovingLeastSquaresOMP<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
{
    // Compute the number of coefficients
    nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;

    #pragma omp parallel for schedule (dynamic, threads_)
    // For all points
    for (int cp = 0; cp < (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))
        {
            if (normals_)
                normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
            continue;
        }

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

        Eigen::Vector4f model_coefficients;
        // Get a plane approximating the local surface's tangent and project point onto it
        computeMLSPointNormal (output.points[cp], *input_, nn_indices, nn_sqr_dists,
                               model_coefficients);

        // Save results to output cloud
        if (normals_)
        {
            normals_->points[cp].normal[0] = model_coefficients[0];
            normals_->points[cp].normal[1] = model_coefficients[1];
            normals_->points[cp].normal[2] = model_coefficients[2];
            normals_->points[cp].curvature = model_coefficients[3];
        }
    }
}
예제 #6
0
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Check if input was set
  if (!normals_)
  {
    ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  if (normals_->points.size () != surface_->points.size ())
  {
    ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset (%zu) differs from the number of points in the dataset containing the normals (%zu)!",
               getClassName ().c_str (), surface_->points.size (), normals_->points.size ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

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

  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);

    // Estimate the principal curvatures at each patch
    computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
                                     output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
                                     output.points[idx].pc1, output.points[idx].pc2);
  }
}
예제 #7
0
파일: mls.hpp 프로젝트: diegodgs/PCL
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);
    }
  }
예제 #8
0
파일: mls.hpp 프로젝트: gimlids/BodyScanner
template <typename PointInT, typename NormalOutT> void
pcl::MovingLeastSquares<PointInT, NormalOutT>::performReconstruction (PointCloudIn &output)
{
  if (search_radius_ <= 0 || sqr_gauss_param_ <= 0)
  {
    PCL_ERROR ("[pcl::%s::performReconstruction] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_);
    output.width = output.height = 0;
    output.points.clear ();
    if (normals_)
    {
      normals_->width = normals_->height = 0;
      normals_->points.clear ();
    }
    return;
  }

  // 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;
  
  // Use original point positions for fitting
  // \note no up/down/adapting-sampling or hole filling possible like this
  output.points.resize (indices_->size ());
  // Check if fake indices were used, otherwise the output loses its organized structure
  if (!fake_indices_)
    pcl::copyPointCloud (*input_, *indices_, output);
  else
    output = *input_;

  // Resize the output normal dataset
  if (normals_)
  {
    normals_->points.resize (output.points.size ());
    normals_->width    = output.width;
    normals_->height   = output.height;
    normals_->is_dense = output.is_dense;
  }

  // For all points
  for (size_t cp = 0; cp < indices_->size (); ++cp)
  {
    // Get the initial estimates of point positions and their neighborhoods
    ///////////////////////////////////////////////////////////////////////

    // Search for the nearest neighbors
    if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
    {
      if (normals_)
        normals_->points[cp].normal[0] = normals_->points[cp].normal[1] = normals_->points[cp].normal[2] = normals_->points[cp].curvature = std::numeric_limits<float>::quiet_NaN ();
      continue;
    }

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

    // Get a plane approximating the local surface's tangent and project point onto it
    //////////////////////////////////////////////////////////////////////////////////

    // Compute the plane coefficients
    Eigen::Vector4f model_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);

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

    // Get the plane normal
    EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
    EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
    pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

    // The normalization is not necessary, since the eigenvectors from libeigen are already normalized
    model_coefficients[0] = eigen_vectors (0, 0);
    model_coefficients[1] = eigen_vectors (1, 0);
    model_coefficients[2] = eigen_vectors (2, 0);
    model_coefficients[3] = 0;
    // Hessian form (D = nc . p_plane (centroid here) + p)
    model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);

    float curvature = 0;
    // Compute the curvature surface change
    float eig_sum = eigen_values.sum ();
    if (eig_sum != 0)
      curvature = fabs (eigen_values[0] / eig_sum);

    // Projected point
    Eigen::Vector3f point = output.points[cp].getVector3fMap ();
    float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
    point -= distance * model_coefficients.head<3> ();

    // Perform polynomial fit to update point and normal
    ////////////////////////////////////////////////////
    if (polynomial_fit_ && k >= nr_coeff_)
    {
      // For easy change between float and double
      typedef Eigen::Vector3d Evector3;
      typedef Eigen::VectorXd Evector;
      typedef Eigen::MatrixXd Ematrix;
      // Get a copy of the plane normal easy access
      Evector3 plane_normal = model_coefficients.head<3> ().cast<double> ();

      // Update neighborhood, since point was projected, and computing relative
      // positions. Note updating only distances for the weights for speed
      std::vector<Evector3> de_meaned (k);
      for (int ni = 0; ni < k; ++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] = de_meaned[ni].dot (de_meaned[ni]);
      }

      // Allocate matrices and vectors to hold the data used for the polynomial
      // fit
      Evector weight_vec_ (k);
      Ematrix P_ (nr_coeff_, k);
      Evector f_vec_ (k);
      Evector c_vec_;
      Ematrix P_weight_; // size will be (nr_coeff_, k);
      Ematrix P_weight_Pt_ (nr_coeff_, nr_coeff_);

      // Get local coordinate system (Darboux frame)
      Evector3 v = plane_normal.unitOrthogonal ();
      Evector3 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 (int ni = 0; ni < k; ++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;
        }
      }

      // Computing coefficients
      P_weight_ = P_ * weight_vec_.asDiagonal ();
      P_weight_Pt_ = P_weight_ * P_.transpose ();
      c_vec_ = P_weight_ * f_vec_;
      P_weight_Pt_.llt ().solveInPlace (c_vec_);

      // Projection onto MLS surface along Darboux normal to the height at (0,0)
      if (pcl_isfinite (c_vec_[0]))
      {
        point += (c_vec_[0] * plane_normal).cast<float> ();

        // Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec_[order_+1] and c_vec_[1]
        if (normals_)
        {
          Evector3 n_a = u + plane_normal * c_vec_[order_ + 1];
          Evector3 n_b = v + plane_normal * c_vec_[1];
          model_coefficients.head<3> () = n_a.cross (n_b).cast<float> ();
          model_coefficients.head<3> ().normalize ();
        }
      }
    }

    // Save results to output cloud
    ///////////////////////////////
    output.points[cp].x = point[0];
    output.points[cp].y = point[1];
    output.points[cp].z = point[2];
    if (normals_)
    {
      normals_->points[cp].normal[0] = model_coefficients[0];
      normals_->points[cp].normal[1] = model_coefficients[1];
      normals_->points[cp].normal[2] = model_coefficients[2];
      normals_->points[cp].curvature = curvature;
    }
  }
}
예제 #9
0
파일: 3dsc.hpp 프로젝트: MorS25/pcl-fuerte
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
    size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
  // The RF is formed as this x_axis | y_axis | normal
  Eigen::Map<Eigen::Vector3f> x_axis (rf);
  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
  Eigen::Map<Eigen::Vector3f> normal (rf + 6);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  if (neighb_cnt == 0)
  {
    for (size_t i = 0; i < desc.size (); ++i)
      desc[i] = std::numeric_limits<float>::quiet_NaN ();

    memset (rf, 0, sizeof (rf[0]) * 9);
    return (false);
  }

  float minDist = std::numeric_limits<float>::max ();
  int minIndex = -1;
  for (size_t i = 0; i < nn_indices.size (); i++)
  {
	  if (nn_dists[i] < minDist)
	  {
      minDist = nn_dists[i];
      minIndex = nn_indices[i];
	  }
  }
  
  // Get origin point
  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
  // Get origin normal
  // Use pre-computed normals
  normal = normals[minIndex].getNormalVector3fMap ();

  // Compute and store the RF direction
  x_axis[0] = rnd ();
  x_axis[1] = rnd ();
  x_axis[2] = rnd ();
  if (!pcl::utils::equal (normal[2], 0.0f))
    x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
  else if (!pcl::utils::equal (normal[1], 0.0f))
    x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
  else if (!pcl::utils::equal (normal[0], 0.0f))
    x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];

  x_axis.normalize ();

  // Check if the computed x axis is orthogonal to the normal
  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));

  // Store the 3rd frame vector
  y_axis = normal.cross (x_axis);

  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal (nn_dists[ne], 0.0f))
		  continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    /// ----- Compute current neighbour polar coordinates -----
    /// Get distance between the neighbour and the origin
    float r = sqrt (nn_dists[ne]); 
    
    /// Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    /// Normalize to compute the dot product
    proj.normalize ();
    
    /// Compute the angle between the projection and the x axis in the interval [0,360] 
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));

    // Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    // Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_+1; rad++) 
    {
      if (r <= radii_interval_[rad]) 
      {
        j = rad-1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_+1; ang++) 
    {
      if (theta <= theta_divisions_[ang]) 
      {
        k = ang-1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_+1; ang++) 
    {
      if (phi <= phi_divisions_[ang]) 
      {
        l = ang-1;
        break;
      }
    }

    // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_distances;
    int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
    // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
    if (point_density == 0)
      continue;

    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) + 
                                                  (k*radius_bins_) + 
                                                  j];
      
    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour

  // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user 
  memset (rf, 0, sizeof (rf[0]) * 9);
  return (true);
}
예제 #10
0
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
  const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
  std::vector<int> n_indices;
  std::vector<float> n_sqr_distances;

  searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);

  Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);

  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();

  double distance = 0.0;
  double sum = 0.0;

  int valid_nn_points = 0;

  for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
  {
    Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
    if (pt.head<3> () == central_point.head<3> ())
		  continue;

    // Difference between current point and origin
    vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
    vij (valid_nn_points, 3) = 0;

    distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);

    // Multiply vij * vij'
    cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());

    sum += distance;
    valid_nn_points++;
  }

  if (valid_nn_points < 5)
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  cov_m /= sum;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);

  const double& e1c = solver.eigenvalues ()[0];
  const double& e2c = solver.eigenvalues ()[1];
  const double& e3c = solver.eigenvalues ()[2];

  if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  // Disambiguation
  Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
  Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
  v1.head<3> () = solver.eigenvectors ().col (2);
  v3.head<3> () = solver.eigenvectors ().col (0);

  int plusNormal = 0, plusTangentDirection1=0;
  for (int ne = 0; ne < valid_nn_points; ne++)
  {
    double dp = vij.row (ne).dot (v1);
    if (dp >= 0)
      plusTangentDirection1++;

    dp = vij.row (ne).dot (v3);
    if (dp >= 0)
      plusNormal++;
  }

  //TANGENT
  plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
  if (plusTangentDirection1 == 0)
  {
		int points = 5; //std::min(valid_nn_points*2/2+1, 11);
		int medianIndex = valid_nn_points/2;

		for (int i = -points/2; i <= points/2; i++)
			if ( vij.row (medianIndex - i).dot (v1) > 0)
				plusTangentDirection1 ++;

		if (plusTangentDirection1 < points/2+1)
			v1 *= - 1;
	} else if (plusTangentDirection1 < 0)
    v1 *= - 1;

  //Normal
  plusNormal = 2*plusNormal - valid_nn_points;
  if (plusNormal == 0)
  {
		int points = 5; //std::min(valid_nn_points*2/2+1, 11);
		int medianIndex = valid_nn_points/2;

		for (int i = -points/2; i <= points/2; i++)
			if ( vij.row (medianIndex - i).dot (v3) > 0)
				plusNormal ++;

		if (plusNormal < points/2+1)
			v3 *= - 1;
	} else if (plusNormal < 0)
    v3 *= - 1;

  rf.row (0) = v1.head<3> ().cast<float> ();
  rf.row (2) = v3.head<3> ().cast<float> ();
  rf.row (1) = rf.row (2).cross (rf.row (0));

  return (0.0f);
}
예제 #11
0
파일: usc.hpp 프로젝트: ShiningPluto/pcl
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
  pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();

  const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
                                frames_->points[index].x_axis[1],
                                frames_->points[index].x_axis[2]);
  //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
  const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
                                frames_->points[index].z_axis[1],
                                frames_->points[index].z_axis[2]);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal(nn_dists[ne], 0.0f))
      continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    // ----- Compute current neighbour polar coordinates -----

    // Get distance between the neighbour and the origin
    float r = std::sqrt (nn_dists[ne]);

    // Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    // Normalize to compute the dot product
    proj.normalize ();

    // Compute the angle between the projection and the x axis in the interval [0,360]
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    /// Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    /// Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
    {
      if (r <= radii_interval_[rad])
      {
        j = rad - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
    {
      if (theta <= theta_divisions_[ang])
      {
        k = ang - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
    {
      if (phi <= phi_divisions_[ang])
      {
        l = ang - 1;
        break;
      }
    }

    /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_didtances;
    float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
    /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
                                                   (k*radius_bins_) +
                                                   j];

    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour
}
예제 #12
0
파일: icp.hpp 프로젝트: jvcleave/ofxPCL
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
  // Allocate enough space to hold the results
  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudTarget input_corresp;
  input_corresp.points.resize (indices_->size ());

  nr_iterations_ = 0;
  converged_ = false;
  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;

  // If the guessed transformation is non identity
  if (guess != Eigen::Matrix4f::Identity ())
  {
    // Initialise final transformation to the guessed one
    final_transformation_ = guess;
    // Apply guessed transformation prior to search for neighbours
    transformPointCloud (output, output, guess);
  }

  // Resize the vector of distances between correspondences 
  std::vector<float> previous_correspondence_distances (indices_->size ());
  correspondence_distances_.resize (indices_->size ());

  while (!converged_)           // repeat until convergence
  {
    // Save the previously estimated transformation
    previous_transformation_ = transformation_;
    // And the previous set of distances
    previous_correspondence_distances = correspondence_distances_;

    int cnt = 0;
    std::vector<int> source_indices (indices_->size ());
    std::vector<int> target_indices (indices_->size ());

    // Iterating over the entire index vector and  find all correspondences
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (!searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
      {
        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
        return;
      }

      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
      if (nn_dists[0] < dist_threshold)
      {
        source_indices[cnt] = (*indices_)[idx];
        target_indices[cnt] = nn_indices[0];
        cnt++;
      }

      // Save the nn_dists[0] to a global vector of distances
      correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold);
    }
    // Resize to the actual number of valid correspondences
    source_indices.resize (cnt); target_indices.resize (cnt);

    std::vector<int> source_indices_good;
    std::vector<int> target_indices_good;
    {
      // From the set of correspondences found, attempt to remove outliers
      // Create the registration model
      typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
      SampleConsensusModelRegistrationPtr model;
      model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
      // Pass the target_indices
      model->setInputTarget (target_, target_indices);
      // Create a RANSAC model
      RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
      sac.setMaxIterations (1000);

      // Compute the set of inliers
      if (!sac.computeModel ())
      {
        source_indices_good = source_indices;
        target_indices_good = target_indices;
      }
      else
      {
        std::vector<int> inliers;
        // Get the inliers
        sac.getInliers (inliers);
        source_indices_good.resize (inliers.size ());
        target_indices_good.resize (inliers.size ());

        boost::unordered_map<int, int> source_to_target;
        for (unsigned int i = 0; i < source_indices.size(); ++i)
          source_to_target[source_indices[i]] = target_indices[i];

        // Copy just the inliers
        std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
        for (size_t i = 0; i < inliers.size (); ++i)
          target_indices_good[i] = source_to_target[inliers[i]];
      }
    }

    // Check whether we have enough correspondences
    cnt = (int)source_indices_good.size ();
    if (cnt < min_number_correspondences_)
    {
      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
      converged_ = false;
      return;
    }

    PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %lu points [100.0%%], RANSAC rejected: %lu [%f%%].\n", getClassName ().c_str (), cnt, (cnt * 100.0) / indices_->size (), (unsigned long)indices_->size (), (unsigned long)source_indices.size () - cnt, (source_indices.size () - cnt) * 100.0 / source_indices.size ());
  
    // Estimate the transform
    //rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
    transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);

    // Tranform the data
    transformPointCloud (output, output, transformation_);

    // Obtain the final transformation    
    final_transformation_ = transformation_ * final_transformation_;

    nr_iterations_++;

    // Update the vizualization of icp convergence
    if (update_visualizer_ != 0)
      update_visualizer_(output, source_indices_good, *target_, target_indices_good );

    // Various/Different convergence termination criteria
    // 1. Number of iterations has reached the maximum user imposed number of iterations (via 
    //    setMaximumIterations)
    // 2. The epsilon (difference) between the previous transformation and the current estimated transformation 
    //    is smaller than an user imposed value (via setTransformationEpsilon)
    // 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via 
    //    setEuclideanFitnessEpsilon)

    if (nr_iterations_ >= max_iterations_ ||
        fabs ((transformation_ - previous_transformation_).sum ()) < transformation_epsilon_ ||
        fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
       )
    {
      converged_ = true;
      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
                 getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ()));

      PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
      PCL_DEBUG ("fabs ((transformation_ - previous_transformation_).sum ()) (%f) < transformation_epsilon_ (%f)\n",
                 fabs ((transformation_ - previous_transformation_).sum ()), transformation_epsilon_);
      PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
                 fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
                 euclidean_fitness_epsilon_);

    }
  }
}
예제 #13
0
파일: mls.hpp 프로젝트: Bardo91/pcl
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;

  typedef typename pcl::traits::fieldList<typename PointCloudIn::PointType>::type FieldListInput;
  typedef typename pcl::traits::fieldList<typename PointCloudOut::PointType>::type FieldListOutput;

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


    /// Copy RGB information if available
    float rgb_input;
    bool rgb_exists_input;
    pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
        input_->points[(*indices_)[cp]], "rgb", rgb_exists_input, rgb_input));

    if (rgb_exists_input)
    {
      for (size_t pp = 0; pp < projected_points.size (); ++pp)
        pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
            projected_points.points[pp], "rgb", rgb_input));
    }

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


  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::Vector3f add_point = distinct_cloud_->points[dp_i].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);

      /// Copy RGB information if available
      float rgb_input;
      bool rgb_exists_input;
      pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
          input_->points[input_index], "rgb", rgb_exists_input, rgb_input));

      if (rgb_exists_input)
      {
          pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
              result_point, "rgb", rgb_input));
      }

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

      /// Copy RGB information if available
      float rgb_input;
      bool rgb_exists_input;
      pcl::for_each_type<FieldListInput> (pcl::CopyIfFieldExists<typename PointCloudIn::PointType, float> (
          input_->points[input_index], "rgb", rgb_exists_input, rgb_input));

      if (rgb_exists_input)
      {
          pcl::for_each_type<FieldListOutput> (pcl::SetIfFieldExists<typename PointCloudOut::PointType, float> (
              result_point, "rgb", rgb_input));
      }

      output.push_back (result_point);
      if (compute_normals_)
        normals_->push_back (result_normal);
    }
  }
}
예제 #14
0
파일: mls.hpp 프로젝트: BITVoyager/pcl
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;

#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
#ifdef _OPENMP
#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
#endif
  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 ().
    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 () >= 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
        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];

        size_t mls_result_index = 0;
        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 = 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

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