C++ PointCloudPtr::at方法代码示例

TEST (GraphCommon, ComputeSignedCurvature)
{
  const size_t ROWS = 5;
  const size_t COLS = 12;
  const float STEP = 1.0;
  // Create a "staircase" cloud
  //
  //     top view            side view
  //
  //   * * * * * * * *           * * * * *
  //   * * * * * * * *           *
  //   * * * * * * * *           *
  //   * * * * * * * *           *
  //   * * * * * * * *     * * * *
  //
  PointCloudPtr cloud (new PointCloud (ROWS * COLS, 1));
  size_t idx = 0;
  for (size_t y = 0; y < ROWS; ++y)
  {
    for (size_t x = 0; x < COLS; ++x, ++idx)
    {
      cloud->at (idx).y = y * STEP;
      if (x < COLS / 3)
      {
        cloud->at (idx).x = x * STEP;
        cloud->at (idx).z = -5.0f;
      }
      else if (x < 2 * COLS / 3)
      {
        cloud->at (idx).x = (COLS / 3 - 1) * STEP;
        cloud->at (idx).z = (x - COLS / 3 + 1) * STEP - 5.0f;
      }
      else
      {
        cloud->at (idx).x = (x - COLS / 3) * STEP;
        cloud->at (idx).z = (COLS / 3) * STEP - 5.0f;
      }
    }
  }

  pcl::graph::NearestNeighborsGraphBuilder<PointT, Graph> gb (6);
  gb.setInputCloud (cloud);
  Graph graph;
  gb.compute (graph);
  pcl::graph::computeNormalsAndCurvatures (graph);

  // Expect that curvatures are all non-negative
  for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
    EXPECT_LE (0.0f, graph[i].curvature);

  pcl::graph::computeSignedCurvatures (graph);

  // Expect curvatures of the points on the concave corner to be less than zero,
  // and on the convex corner the other way round.
  for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
    if (i % COLS == 3)
      EXPECT_GT (0.0f, graph[i].curvature);
    else if (i % COLS == 7)
      EXPECT_LT (0.0f, graph[i].curvature);
}

//if a point is in a cloud
bool isPointInCloud(Point pt, PointCloudPtr cloud){
  for(int i=0;i<cloud->size();i++){
    if(pt.x==cloud->at(i).x&&pt.y==cloud->at(i).y&&pt.z==cloud->at(i).z){
      return true;
    }
  }

  return false;
}

//find nearest neighbor
bool findNearestNeighbor(PointCloudPtr cloud, PointCloudPtr except_cloud, Point search_pt, Point& finded_pt){
  int min=10;
  bool flag=false;

  for(int i=0;i<cloud->size();i++){
    int dis=sqrt(pow(search_pt.x-cloud->at(i).x, 2)+pow(search_pt.y-cloud->at(i).y, 2)+pow(search_pt.z-cloud->at(i).z, 2));
    if(dis<min){
      min=dis;
      finded_pt.x=cloud->at(i).x;
      finded_pt.y=cloud->at(i).y;
      finded_pt.z=cloud->at(i).z;

      if(!isPointInCloud(finded_pt, except_cloud)){
        flag=true;
      }
    }
  }

  return flag;
}

template<typename PointInT> std::size_t
pcl::nurbs::NurbsFitter<PointInT>::PCL2Eigen (PointCloudPtr &pcl_cloud, const std::vector<int> &indices, vector_vec3 &on_cloud)
{
  std::size_t numPoints (0);

  for (unsigned i = 0; i < indices.size (); i++)
  {
    const PointInT &pt = pcl_cloud->at (indices[i]);

    if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
    {
      on_cloud.push_back (vec3 (pt.x, pt.y, pt.z));
      ++numPoints;
    }
  }

  return (numPoints);
}

TEST (GraphCommon, CreateSubgraphsFromIndicesSingle)
{
  const size_t N = 100;
  PointCloudPtr cloud = generateRandomPlanarCloud (N);
  Subgraph graph (cloud);

  pcl::PointIndices indices;
  for (size_t i = 0; i < N; ++i)
    if (i % 10)
      indices.indices.push_back (i);

  std::vector<SubgraphRef> subgraphs;
  pcl::graph::createSubgraphsFromIndices (graph, indices, subgraphs);
  ASSERT_EQ (2, subgraphs.size ());
  ASSERT_EQ (indices.indices.size (), boost::num_vertices (subgraphs[0].get ()));
  ASSERT_EQ (N - indices.indices.size (), boost::num_vertices (subgraphs[1].get ()));
  for (size_t i = 0; i < boost::num_vertices (subgraphs[0].get ()); ++i)
    EXPECT_XYZ_EQ (cloud->at (indices.indices[i]), subgraphs[0].get ()[i]);
}

void ppfmap::CudaPPFMatch<PointT, NormalT>::detect(
    const PointCloudPtr cloud, 
    const NormalsPtr normals, 
    std::vector<Pose>& poses) {

    float affine_s[12];
    std::vector<Pose> pose_vector;
    const float radius = map->getCloudDiameter() * neighborhood_percentage;

    std::vector<int> indices;
    std::vector<float> distances;

    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(cloud);

    if (!use_indices) {
        ref_point_indices->resize(cloud->size());
        for (int i = 0; i < cloud->size(); i++) {
            (*ref_point_indices)[i] = i;
        }
    }

    poses.clear();
    for (const auto index : *ref_point_indices) {
        const auto& point = cloud->at(index);
        const auto& normal = normals->at(index);

        if (!pcl::isFinite(point)) continue;

        getAlignmentToX(point, normal, &affine_s);
        kdtree.radiusSearch(point, radius, indices, distances);

        poses.push_back(getPose(index, indices, cloud, normals, affine_s));
    }

    sort(poses.begin(), poses.end(), 
         [](const Pose& a, const Pose& b) -> bool { 
             return a.votes > b.votes; 
         });
}

ppfmap::Pose ppfmap::CudaPPFMatch<PointT, NormalT>::getPose(
    const int reference_index,
    const std::vector<int>& indices,
    const PointCloudPtr cloud,
    const NormalsPtr normals,
    const float affine_s[12]) {

    Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tsg_map(affine_s);

    float affine_m[12];
    const std::size_t n = indices.size();

    const auto& ref_point = cloud->at(reference_index);
    const auto& ref_normal = normals->at(reference_index);

    thrust::host_vector<uint32_t> hash_list(n);
    thrust::host_vector<float> alpha_s_list(n);

    // Compute the PPF feature for all the pairs in the neighborhood
    for (int i = 0; i < n; i++) {
        const int index = indices[i];
        const auto& point = cloud->at(index);
        const auto& normal = normals->at(index);

        // Compute the PPF between reference_point and the i-th neighbor
        hash_list[i] = computePPFFeatureHash(ref_point, ref_normal,
                                             point, normal,
                                             distance_step,
                                             angle_step);

        // Compute the alpha_s angle
        const Eigen::Vector3f transformed(Tsg_map * point.getVector4fMap());
        alpha_s_list[i] = atan2f(-transformed(2), transformed(1));

    }

    int index;
    float alpha;
    int votes;

    map->searchBestMatch(hash_list, alpha_s_list, index, alpha, votes);

    const auto& model_point = model_->at(index);
    const auto& model_normal = normals_->at(index);

    getAlignmentToX(model_point, model_normal, &affine_m);

    Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tmg_map(affine_m);

    Eigen::Affine3f Tsg, Tmg;
    Tsg.matrix().block<3, 4>(0, 0) = Tsg_map.matrix();
    Tmg.matrix().block<3, 4>(0, 0) = Tmg_map.matrix();

    // Set final pose
    Pose final_pose;
    final_pose.c = pcl::Correspondence(reference_index, index, 0.0f);
    final_pose.t = Tsg.inverse() * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX()) * Tmg;
    final_pose.votes = votes;

    return final_pose;
}

//append a cloud to another cloud
void appendCloud(PointCloudPtr sourceCloud,PointCloudPtr targetCloud){
  for(int i=0;i<sourceCloud->size();i++){
    targetCloud->push_back(sourceCloud->at(i));
  }
}