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

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

//.........这里部分代码省略.........
        std::vector<float> r (4,0);

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

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

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

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

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

        if (d < first_threshold_) continue;

        std::vector<float> B (4,0);
        std::vector<float> A (4,0);
        std::vector<float> sumAB (4,0);
        B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
        B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
        B[1] = normalsDiff (right, upright) * normalsDiff (upright, center);
        B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center);
        B[2] = normalsDiff (downright, right) * normalsDiff (downright, center);
        B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center);
        B[3] = normalsDiff (down, downright) * normalsDiff (downright, center);
        B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center);
        A[0] = r[1] - r[0] - B[0] - B[0];
        A[1] = r[2] - r[1] - B[1] - B[1];
        A[2] = r[3] - r[2] - B[2] - B[2];
        A[3] = r[0] - r[3] - B[3] - B[3];
        sumAB[0] = A[0] + B[0];
        sumAB[1] = A[1] + B[1];
        sumAB[2] = A[2] + B[2];
        sumAB[3] = A[3] + B[3];
        if ((*std::max_element (B.begin (), B.end ()) < 0) &&
            (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
        {
          std::vector<float> D (4,0);
          D[0] = B[0] * B[0] / A[0];
          D[1] = B[1] * B[1] / A[1];
          D[2] = B[2] * B[2] / A[2];
          D[3] = B[3] * B[3] / A[3];
          response (i,j) = *(std::min (D.begin (), D.end ()));
        }
        else
          response (i,j) = d;
      }
    }
  }
  // Non maximas suppression
  std::vector<int> indices = *indices_;
  std::sort (indices.begin (), indices.end (),
             boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2));

  output.clear ();
  output.reserve (input_->size ());

  std::vector<bool> occupency_map (indices.size (), false);
  const int width (input_->width);
  const int height (input_->height);
  const int occupency_map_size (indices.size ());

#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads (threads_)
#endif
  for (int i = 0; i < indices.size (); ++i)
  {
    int idx = indices[i];
    if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
      continue;

    PointOutT p;
    p.getVector3fMap () = input_->points[idx].getVector3fMap ();
    p.intensity = response_->points [idx];

#ifdef _OPENMP
#pragma omp critical
#endif
    {
      output.push_back (p);
      keypoints_indices_->indices.push_back (idx);
    }

    const int x = idx % width;
    const int y = idx / width;
    const int u_end = std::min (width, x + half_window_size_);
    const int v_end = std::min (height, y + half_window_size_);
    for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
      for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
        occupency_map[v*width + u] = true;
  }

  output.height = 1;
  output.width = static_cast<uint32_t> (output.size());
  // we don not change the denseness
  output.is_dense = true;
}

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

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

  switch (upsample_method_)
  {
    case (NONE):
    {
      Eigen::Vector3d normal = plane_normal;

      if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && 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 (compute_normals_)
          normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
      }

      PointOutT aux;
      aux.x = point[0];
      aux.y = point[1];
      aux.z = point[2];
      projected_points.push_back (aux);

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

      break;
    }

    case (SAMPLE_LOCAL_PLANE):
    {
      // Uniformly sample a circle around the query point using the radius and step parameters
      for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
        for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
          if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
          {
            PointOutT projected_point;
            pcl::Normal projected_normal;
            projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec, 
                                      static_cast<int> (nn_indices.size ()),
                                      projected_point, projected_normal);

            projected_points.push_back (projected_point);
            if (compute_normals_)
              projected_points_normals.push_back (projected_normal);
          }
      break;

//.........这里部分代码省略.........

// #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)
    {
      if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx]))
        continue;
        
#ifdef _OPENMP
#pragma omp critical
#endif
      output.push_back (response_->at (indices_->at (idx)));
      
			int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
			int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
      for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
        for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
          occupency_map[v*input_->width+u] = true;
    }

    // if (refine_)
    //   refineCorners (output);

    output.height = 1;
    output.width = static_cast<uint32_t> (output.size());
  }

  // we don not change the denseness
  output.is_dense = input_->is_dense;
}

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

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