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

template <typename VoxelT, typename WeightT> void
pcl::TSDFVolume<VoxelT, WeightT>::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
{
  int sx = header_.resolution(0);
  int sy = header_.resolution(1);
  int sz = header_.resolution(2);

  const int step = 2;
  const int cloud_size = header_.getVolumeSize() / (step*step*step);

  cloud->clear();
  cloud->reserve (std::min (cloud_size/10, 500000));

  int volume_idx = 0, cloud_idx = 0;
//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
  for (int z = 0; z < sz; z+=step)
    for (int y = 0; y < sy; y+=step)
      for (int x = 0; x < sx; x+=step, ++cloud_idx)
      {
        volume_idx = sx*sy*z + sx*y + x;
        // pcl::PointXYZI &point = cloud->points[cloud_idx];

        if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
          continue;

        pcl::PointXYZI point;
        point.x = x; point.y = y; point.z = z;//*64;
        point.intensity = volume_->at(volume_idx);
        cloud->push_back (point);
      }

  // cloud->width = cloud_size;
  // cloud->height = 1;
}

void DownSampler::downSamplePointsStochastic(
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& points,
    const int target_num_points,
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr& down_sampled_points)
{
  const size_t num_points = points->size();

  // Check if the points are already sufficiently down-sampled.
  if (target_num_points >= num_points * 0.8){
    *down_sampled_points = *points;
    return;
  }

  // Allocate space for the new points.
  down_sampled_points->reserve(target_num_points);

  //Just to ensure that we don't end up with 0 points, add 1 point to this
  down_sampled_points->push_back((*points)[0]);

  // Randomly select points with (targetNumPoints / num_points) probability.
  for (size_t i = 1; i < num_points; ++i){
    const pcl::PointXYZRGB& pt = (*points)[i];
    if (rand() % num_points < target_num_points){
      down_sampled_points->push_back(pt);
    }
  }
}

template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getCentroidCloud (pcl::PointCloud<PointSuperVoxel>::Ptr &cloud )
{
  bool use_existing_points = true;
  if ((cloud == 0) || (cloud->size () != this->OctreeBaseT::getLeafCount ()))
  {
    cloud.reset ();
    cloud = boost::make_shared<pcl::PointCloud<PointSuperVoxel> > ();
    cloud->reserve (this->OctreeBaseT::getLeafCount ());
    use_existing_points = false;
  }
  typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
  leaf_itr = this->leaf_begin ();
  
  LeafContainerT* leafContainer;
  PointSuperVoxel point;
  for ( int index = 0; leaf_itr != this->leaf_end (); ++leaf_itr,++index)
  {
    leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
    if (!use_existing_points)
    {
      leafContainer->getCentroid (point);
      cloud->push_back (point);
    }
    else
    {
      leafContainer->getCentroid (cloud->points[index]);
    }
    
  }  
}

void DownSampler::downSamplePointsDeterministic(
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& points,
    const int target_num_points,
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr& down_sampled_points,
    const bool use_ceil)
{
  const size_t num_points = points->size();

  // Check if the points are already sufficiently down-sampled.
  if (target_num_points >= num_points * 0.8){
    *down_sampled_points = *points;
    return;
  }

  // Select every N points to reach the target number of points.
  int everyN = 0;
  if (use_ceil) {
    everyN = ceil(static_cast<double>(num_points) /
                  static_cast<double>(target_num_points));
  } else {
    everyN = static_cast<double>(num_points) /
        static_cast<double>(target_num_points);
  }

  // Allocate space for the new points.
  down_sampled_points->reserve(target_num_points);

  //Just to ensure that we don't end up with 0 points, add 1 point to this
  down_sampled_points->push_back((*points)[0]);

  // Select every N points to reach the target number of points.
  for (size_t i = 1; i < num_points; ++i) {
    if (i % everyN == 0){
      const pcl::PointXYZRGB& pt = (*points)[i];
      down_sampled_points->push_back(pt);
    }
  }
}