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

  void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius,
  typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];
    const Eigen::Vector3f ept(pt.x,pt.y,pt.z);

    if ((ept - center).squaredNorm() > radius * radius)
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  uint count = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);
      count++;
    }
  out_cloud->resize(count);
}

  void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max,
  typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];

    if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() ||
      pt.x < min.x() || pt.y < min.y() || pt.z < min.z())
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  uint count = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);
      count++;
    }
  out_cloud->resize(count);
}

template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const
{
  normals.reset (new pcl::PointCloud<Normal>);
  normals->clear ();
  normals->resize (leaves_.size ());
  typename SupervoxelHelper::const_iterator leaf_itr;
  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getNormal (*normal_itr);
  }
}

template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const
{
  normals = boost::make_shared<pcl::PointCloud<Normal> > ();
  normals->clear ();
  normals->resize (leaves_.size ());
  typename std::set<LeafContainerT*>::iterator leaf_itr;
  typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
  for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getNormal (*normal_itr);
  }
}

template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const
{
  voxels = boost::make_shared<pcl::PointCloud<PointT> > ();
  voxels->clear ();
  voxels->resize (leaves_.size ());
  typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin ();
  //typename std::set<LeafContainerT*>::iterator leaf_itr;
  for (typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin (); 
	  leaf_itr != leaves_.end (); 
	  ++leaf_itr, ++voxel_itr)
  {
    const VoxelData& leaf_data = (*leaf_itr)->getData ();
    leaf_data.getPoint (*voxel_itr);
  }
}

  void WorldDownloadManager::removeDuplicatePoints(typename pcl::PointCloud<PointT>::ConstPtr cloud,
  TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud,TrianglesPtr out_triangles)
{
  const uint64 input_size = cloud->size();
  std::vector<uint64> ordered(input_size);
  for (uint64 i = 0; i < input_size; i++)
    ordered[i] = i;

  std::sort(ordered.begin(),ordered.end(),WorldDownloadManager_mergeMeshTrianglesComparator<PointT>(*cloud));

  typedef WorldDownloadManager_mergeMeshTrianglesAverageClass<PointT> AverageClass;
  std::vector<uint64> re_association_vector(input_size);
  std::vector<AverageClass, Eigen::aligned_allocator<AverageClass> > average_vector;

  uint64 output_i = 0;
  re_association_vector[ordered[0]] = 0;

  for (uint64 src_i = 1; src_i < input_size; src_i++)
  {
    const PointT & prev = (*cloud)[ordered[src_i - 1]];
    const PointT & current = (*cloud)[ordered[src_i]];
    const int not_equal = std::memcmp(prev.data,current.data,sizeof(float) * 3);
    if (not_equal)
      output_i++;
    re_association_vector[ordered[src_i]] = output_i;
  }
  const uint64 output_size = output_i + 1;

  out_cloud->resize(output_size);
  average_vector.resize(output_size);
  for (uint64 i = 0; i < input_size; i++)
    average_vector[re_association_vector[i]].insert((*cloud)[i]);
  for (uint64 i = 0; i < output_size; i++)
    (*out_cloud)[i] = average_vector[i].get();

  if (out_triangles && triangles)
  {
    const uint64 triangles_size = triangles->size();
    uint64 out_triangles_size = 0;
    out_triangles->resize(triangles_size);
    for (uint64 i = 0; i < triangles_size; i++)
    {
      kinfu_msgs::KinfuMeshTriangle new_tri;
      for (uint h = 0; h < 3; h++)
        new_tri.vertex_id[h] = re_association_vector[(*triangles)[i].vertex_id[h]];

      bool degenerate = false;
      for (uint h = 0; h < 3; h++)
        if (new_tri.vertex_id[h] == new_tri.vertex_id[(h + 1) % 3])
          degenerate = true;

      if (!degenerate)
      {
        (*out_triangles)[out_triangles_size] = new_tri;
        out_triangles_size++;
      }
    }

    out_triangles->resize(out_triangles_size);
  }
}

void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
  const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud,
  TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles)
{
  const uint triangles_size = triangles->size();
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  std::vector<uint> valid_points_remap(cloud_size,0);

  uint offset;

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];

    if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
      pt.x < min.x || pt.y < min.y || pt.z < min.z)
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  offset = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);

      // save new position for triangles remap
      valid_points_remap[i] = offset;

      offset++;
    }
  out_cloud->resize(offset);

  // discard invalid triangles
  out_triangles->clear();
  out_triangles->reserve(triangles_size);
  offset = 0;

  for (uint i = 0; i < triangles_size; i++)
  {
    const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
    bool is_valid = true;

    // validate all the vertices
    for (uint h = 0; h < 3; h++)
      if (!valid_points[tri.vertex_id[h]])
      {
        is_valid = false;
        break;
      }

    if (is_valid)
    {
      kinfu_msgs::KinfuMeshTriangle out_tri;

      // remap the triangle
      for (uint h = 0; h < 3; h++)
        out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];

      out_triangles->push_back(out_tri);
      offset++;
    }

  }
  out_triangles->resize(offset);
}