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

 PcdGtAnnotator()
 {
     f_ = 525.f;
     source_.reset(new v4r::ModelOnlySource<pcl::PointXYZRGBNormal, pcl::PointXYZRGB>);
     models_dir_ = "";
     gt_dir_ = "";
     threshold_ = 0.01f;
     first_view_only_ = false;
     reconstructed_scene_.reset(new pcl::PointCloud<PointT>);
 }

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

void
NMBasedCloudIntegration<PointT>::compute (typename pcl::PointCloud<PointT>::Ptr & output)
{
    if(input_clouds_.empty())
    {
        LOG(ERROR) << "No input clouds set for cloud integration!";
        return;
    }

    big_cloud_info_.clear();

    collectInfo();

    if(param_.reason_about_points_)
        reasonAboutPts();

    pcl::octree::OctreePointCloudPointVector<PointT> octree( param_.octree_resolution_ );
    typename pcl::PointCloud<PointT>::Ptr big_cloud ( new pcl::PointCloud<PointT>());
    big_cloud->width = big_cloud_info_.size();
    big_cloud->height = 1;
    big_cloud->points.resize( big_cloud_info_.size() );
    for(size_t i=0; i < big_cloud_info_.size(); i++)
        big_cloud->points[i] = big_cloud_info_[i].pt;
    octree.setInputCloud( big_cloud );
    octree.addPointsFromInputCloud();

    typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator leaf_it;
    const typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator it2_end = octree.leaf_end();

    size_t kept = 0;
    size_t total_used = 0;

    std::vector<PointInfo> filtered_cloud_info ( big_cloud_info_.size() );

    for (leaf_it = octree.leaf_begin(); leaf_it != it2_end; ++leaf_it)
    {
        pcl::octree::OctreeContainerPointIndices& container = leaf_it.getLeafContainer();

        // add points from leaf node to indexVector
        std::vector<int> indexVector;
        container.getPointIndices (indexVector);

        if(indexVector.empty())
            continue;

        std::vector<PointInfo> voxel_pts ( indexVector.size() );

        for(size_t k=0; k < indexVector.size(); k++)
            voxel_pts[k] = big_cloud_info_ [indexVector[k]];

        PointInfo p;

        size_t num_good_pts = 0;
        if(param_.average_)
        {
            for(const PointInfo &pt_tmp : voxel_pts)
            {
                if (pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
                {
                    p.moving_average( pt_tmp );
                    num_good_pts++;
                }
            }

            if(  num_good_pts < param_.min_points_per_voxel_ )
                continue;

            total_used += num_good_pts;
        }
        else // take only point with min weight
        {
            for(const PointInfo &pt_tmp : voxel_pts)
            {
                if ( pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
                {
                    num_good_pts++;
                    if ( pt_tmp.weight < p.weight || num_good_pts == 1)
                        p = pt_tmp;
                }
            }

            if( num_good_pts < param_.min_points_per_voxel_ )
                continue;

            total_used++;
        }
        filtered_cloud_info[kept++] = p;
    }

    LOG(INFO) << "Number of points in final noise model based integrated cloud: " << kept << " used: " << total_used << std::endl;


    if(!output)
        output.reset(new pcl::PointCloud<PointT>);

    if(!output_normals_)
        output_normals_.reset( new pcl::PointCloud<pcl::Normal>);

    filtered_cloud_info.resize(kept);
    output->points.resize(kept);
//.........这里部分代码省略.........

 View()
 {
     cloud_.reset(new pcl::PointCloud<PointT>());
 }

  static bool colorize(const drc::map_image_t& iDepthMap,
                       const bot_core::image_t& iImage,
                       typename pcl::PointCloud<T>::Ptr& oCloud) {
    int w(iDepthMap.width), h(iDepthMap.height);
    if ((w != iImage.width) || (h != iImage.height)) {
      return false;
    }

    // TODO: for now, reject compressed depth maps
    if (iDepthMap.compression != drc::map_image_t::COMPRESSION_NONE) {
      return false;
    }

    // TODO: for now, only accept rgb3 images
    if (iImage.pixelformat != PIXEL_FORMAT_RGB) {
      return false;
    }

    if (oCloud == NULL) {
      oCloud.reset(new pcl::PointCloud<T>());
    }
    oCloud->points.clear();

    Eigen::Matrix4d xform;
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        xform(i,j) = iDepthMap.transform[i][j];
      }
    }
    xform = xform.inverse();

    for (int i = 0; i < h; ++i) {
      for (int j = 0; j < w; ++j) {
        double z;
        if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
          z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
          if (z < -1e10) {
            continue;
          }
        }
        else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
          uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
          if (val == 0) {
            continue;
          }
          z = val;
        }
        else {
          continue;
        }

        Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
        T newPoint;
        newPoint.x = pt(0)/pt(3);
        newPoint.y = pt(1)/pt(3);
        newPoint.z = pt(2)/pt(3);
        int imageIndex = i*iImage.row_stride + 3*j;
        newPoint.r = iImage.data[imageIndex+0];
        newPoint.g = iImage.data[imageIndex+1];
        newPoint.b = iImage.data[imageIndex+2];
        oCloud->points.push_back(newPoint);
      }
    }
    oCloud->width = oCloud->points.size();
    oCloud->height = 1;
    oCloud->is_dense = false;

    return true;
  }