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

void App::Load() {
    Input_.reset(new PointCloud);
    pcl::io::loadPCDFile(InputPath_, *Input_);
}

void GraphicEnd::downsamplePointCloud(PointCloud::Ptr& pc_in,PointCloud::Ptr& pc_downsampled)
{
    if(use_voxel)
    {
        pcl::VoxelGrid<pcl::PointXYZRGB> grid;
        grid.setLeafSize(0.05,0.05,0.05);
        grid.setFilterFieldName ("z");
        grid.setFilterLimits (0.0,5.0);

        grid.setInputCloud(pc_in);
        grid.filter(*pc_downsampled);
    }
    else
    {
        int downsamplingStep=8;
        static int j;j=0;
        std::vector<double> xV;
        std::vector<double> yV;
        std::vector<double> zV;
        std::vector<double> rV;
        std::vector<double> gV;
        std::vector<double> bV;

        pc_downsampled.reset(new pcl::PointCloud<pcl::PointXYZRGB> );
        pc_downsampled->points.resize(640*480/downsamplingStep*downsamplingStep);
        for(int r=0;r<480;r=r+downsamplingStep)
        {
            for(int c=0;c<640;c=c+downsamplingStep)
            {
                int nPoints=0;
                xV.resize(downsamplingStep*downsamplingStep);
                yV.resize(downsamplingStep*downsamplingStep);
                zV.resize(downsamplingStep*downsamplingStep);
                rV.resize(downsamplingStep*downsamplingStep);
                gV.resize(downsamplingStep*downsamplingStep);
                bV.resize(downsamplingStep*downsamplingStep);
                
                for(int r2=r;r2<r+downsamplingStep;r2++)
                {
                    for(int c2=c;c2<c+downsamplingStep;c2++)
                    {
                        //Check if the point has valid data
                        if(pcl_isfinite (pc_in->points[r2*640+c2].x) &&
                           pcl_isfinite (pc_in->points[r2*640+c2].y) &&
                           pcl_isfinite (pc_in->points[r2*640+c2].z) &&
                           0.3<pc_in->points[r2*640+c2].z &&
                           pc_in->points[r2*640+c2].z<5)
                        {
                            //Create a vector with the x, y and z coordinates of the square region and RGB info
                            xV[nPoints]=pc_in->points[r2*640+c2].x;
                            yV[nPoints]=pc_in->points[r2*640+c2].y;
                            zV[nPoints]=pc_in->points[r2*640+c2].z;
                            rV[nPoints]=pc_in->points[r2*640+c2].r;
                            gV[nPoints]=pc_in->points[r2*640+c2].g;
                            bV[nPoints]=pc_in->points[r2*640+c2].b;
                            
                            nPoints++;
                        }
                    }
                }
                
                if(nPoints>0)
                {
                    xV.resize(nPoints);
                    yV.resize(nPoints);
                    zV.resize(nPoints);
                    rV.resize(nPoints);
                    gV.resize(nPoints);
                    bV.resize(nPoints);
                    
                    //Compute the mean 3D point and mean RGB value
                    std::sort(xV.begin(),xV.end());
                    std::sort(yV.begin(),yV.end());
                    std::sort(zV.begin(),zV.end());
                    std::sort(rV.begin(),rV.end());
                    std::sort(gV.begin(),gV.end());
                    std::sort(bV.begin(),bV.end());
                    
                    pcl::PointXYZRGB point;
                    point.x=xV[nPoints/2];
                    point.y=yV[nPoints/2];
                    point.z=zV[nPoints/2];
                    point.r=rV[nPoints/2];
                    point.g=gV[nPoints/2];
                    point.b=bV[nPoints/2];
                    
                    //Set the mean point as the representative point of the region
                    pc_downsampled->points[j]=point;
                    j++;
                }
            }
        }
        pc_downsampled->points.resize(j);
        pc_downsampled->width=pc_downsampled->size();
        pc_downsampled->height=1;
    }
}

std::vector<PointCloud> TableClusterDetector::findTableClusters(const sensor_msgs::PointCloud2 &scene)
{

  std::vector<PointCloud> clusters;

  // Convert the dataset
  PointCloud cloud; PointCloud::Ptr cloudPtr;
  pcl::fromROSMsg (scene, cloud);
  cloudPtr.reset(new PointCloud(cloud));

  // Remove NaNs
  PointCloud cloud_filtered;
  pass_.setInputCloud (cloudPtr);
  pass_.filter (cloud_filtered);

  cloudPtr.reset(new PointCloud(cloud_filtered));
  // Downsample
  PointCloud cloud_downsampled;
  grid_.setInputCloud (cloudPtr);
  grid_.filter (cloud_downsampled);
  cloudPtr.reset(new PointCloud(cloud_downsampled));

  if ((int)cloud_filtered.points.size() < k_)
  {
    ROS_WARN("Filtering returned %zd points! Skipping.", cloud_filtered.points.size());
    return clusters;
  }

  // Estimate the point normals
  pcl::PointCloud<pcl::Normal> cloud_normals;pcl::PointCloud<pcl::Normal>::Ptr cloud_normalsPtr;
  // add this if normal estimation is inaccurate
  //n3d_.setSearchSurface (cloud_);
  n3d_.setInputCloud (cloudPtr);
  n3d_.compute (cloud_normals);
  cloud_normalsPtr.reset(new pcl::PointCloud<pcl::Normal>(cloud_normals));
  ROS_INFO ("[TableObjectDetector] %d normals estimated.", (int)cloud_normals.points.size ());


  // ---[ Perform segmentation
  pcl::PointIndices table_inliers; pcl::PointIndices::Ptr table_inliersPtr;
  pcl::ModelCoefficients table_coefficients; pcl::ModelCoefficients::Ptr table_coefficientsPtr;
  seg_.setInputCloud (cloudPtr);
  seg_.setInputNormals (cloud_normalsPtr);
  seg_.segment (table_inliers, table_coefficients);
  table_inliersPtr = boost::make_shared<pcl::PointIndices>(table_inliers);
  table_coefficientsPtr = boost::make_shared<pcl::ModelCoefficients>(table_coefficients);

  if (table_coefficients.values.size () > 3)
    ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers.indices.size (),
        table_coefficients.values[0], table_coefficients.values[1], table_coefficients.values[2], table_coefficients.values[3]);

  if (table_inliers.indices.size () == 0)
    return clusters;

  // ---[ Extract the table
  PointCloud table_projected; PointCloud::Ptr table_projectedPtr;
  proj_.setInputCloud (cloudPtr);
  proj_.setIndices (table_inliersPtr);
  proj_.setModelCoefficients (table_coefficientsPtr);
  proj_.filter (table_projected);
  table_projectedPtr.reset (new PointCloud(table_projected));
  ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ());

  sensor_msgs::PointCloud table_points;
  tf::Transform table_plane_trans = getPlaneTransform (*table_coefficientsPtr, -1.0);
  std::string base_frame_id = scene.header.frame_id;
  ROS_INFO("sending table transform");
  ros::Rate r(10);
  for (int i=0; i < 10; i++) {
    tf_pub_.sendTransform(tf::StampedTransform(table_plane_trans, ros::Time::now(), 
          base_frame_id, "table"));
    r.sleep();
  }
  //takes the points projected on the table and transforms them into the PointCloud message
  //while also transforming them into the table's coordinate system
  getPlanePoints<Point> (table_projected, table_plane_trans, table_points);
  ROS_INFO("Table computed");

  table_ = computeTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points);
  publishTable(table_);
  // ---[ Estimate the convex hull
  PointCloud table_hull; PointCloud::Ptr table_hullPtr;
  hull_.setInputCloud (table_projectedPtr);
  hull_.reconstruct (table_hull);
  table_hullPtr.reset (new PointCloud(table_hull));

  // ---[ Get the objects on top of the table
  pcl::PointIndices cloud_object_indices; pcl::PointIndices::Ptr cloud_object_indicesPtr;
  prism_.setInputCloud (cloudPtr);
  prism_.setInputPlanarHull (table_hullPtr);
  prism_.segment (cloud_object_indices);
  cloud_object_indicesPtr = boost::make_shared<pcl::PointIndices>(cloud_object_indices);
  ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ());

  PointCloud cloud_objects; PointCloud::Ptr cloud_objectsPtr;
  pcl::ExtractIndices<Point> extract_object_indices;
  extract_object_indices.setInputCloud (cloudPtr);
  extract_object_indices.setIndices (cloud_object_indicesPtr);
  extract_object_indices.filter (cloud_objects);
  cloudPtr.reset(new PointCloud(cloud_objects));
//.........这里部分代码省略.........