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

  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::mergePointCloudsAndMesh(std::vector<typename pcl::PointCloud<PointT>::Ptr> &pointclouds,
  typename pcl::PointCloud<PointT>::Ptr out_cloud, std::vector<TrianglesPtr> * meshes,Triangles * out_mesh)
{
  uint offset = 0;
  const uint pointcloud_count = pointclouds.size();

  out_cloud->clear();

  if (out_mesh)
    out_mesh->clear();

  for (uint pointcloud_i = 0; pointcloud_i < pointcloud_count; pointcloud_i++)
  {
    const uint pointcloud_size = pointclouds[pointcloud_i]->size();

    // copy the points
    (*out_cloud) += *(pointclouds[pointcloud_i]);

    if (out_mesh)
    {
      // copy the triangles, shifting vertex id by an offset
      const uint mesh_size = (*meshes)[pointcloud_i]->size();
      out_mesh->reserve(out_mesh->size() + mesh_size);

      for (uint triangle_i = 0; triangle_i < mesh_size; triangle_i++)
      {
        kinfu_msgs::KinfuMeshTriangle tri;
        const kinfu_msgs::KinfuMeshTriangle & v = (*(*meshes)[pointcloud_i])[triangle_i];

        for (uint i = 0; i < 3; i++)
          tri.vertex_id[i] = v.vertex_id[i] + offset;

        out_mesh->push_back(tri);
      }

      offset += pointcloud_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);
}

template <typename PointT> void
pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon,
                           typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
                           float threshold, bool refine, bool closed)
{
    approx_polygon.clear ();
    if (polygon.size () < 3)
        return;

    std::vector<std::pair<unsigned, unsigned> > intervals;
    std::pair<unsigned,unsigned> interval (0, 0);

    if (closed)
    {
        float max_distance = .0f;
        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
                             (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.second = idx;
            }
        }

        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
                             (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.first = idx;
            }
        }

        if (max_distance < threshold * threshold)
            return;

        intervals.push_back (interval);
        std::swap (interval.first, interval.second);
        intervals.push_back (interval);
    }
    else
    {
        interval.first = 0;
        interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
        intervals.push_back (interval);
    }

    std::vector<unsigned> result;
    // recursively refine
    while (!intervals.empty ())
    {
        std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
        float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
        float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
        float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;

        float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);

        line_x *= linelen;
        line_y *= linelen;
        line_d *= linelen;

        float max_distance = 0.0;
        unsigned first_index = currentInterval.first + 1;
        unsigned max_index = 0;

        // => 0-crossing
        if (currentInterval.first > currentInterval.second)
        {
            for (unsigned idx = first_index; idx < polygon.size(); idx++)
            {
                float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
                if (distance > max_distance)
                {
                    max_distance = distance;
                    max_index  = idx;
                }
            }
            first_index = 0;
        }

        for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
        {
            float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
            if (distance > max_distance)
            {
                max_distance = distance;
                max_index  = idx;
            }
        }

        if (max_distance > threshold)
        {
            std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
//.........这里部分代码省略.........