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

void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){

  if (cloud->isOrganized()) {
    std::cout << "PointCloud is organized..." << std::endl;

    result = cv::Mat(cloud->height, cloud->width, CV_8UC3);

    if (!cloud->empty()) {

      for (int h=0; h<result.rows; h++) {
        for (int w=0; w<result.cols; w++) {
            PointT point = cloud->at(w, h);

            Eigen::Vector3i rgb = point.getRGBVector3i();

            result.at<cv::Vec3b>(h,w)[0] = rgb[2];
            result.at<cv::Vec3b>(h,w)[1] = rgb[1];
            result.at<cv::Vec3b>(h,w)[2] = rgb[0];
        }
      }
    }
  }
}

void filter_PC_from_BB(PointCloudT::Ptr &cloud, cv::Mat &result, int x, int y, int width, int height){

  const float bad_point = std::numeric_limits<float>::quiet_NaN();

  if (cloud->isOrganized()) {
    std::cout << "PointCloud is organized..." << std::endl;
    result = cv::Mat(cloud->height, cloud->width, CV_8UC3);

    if (!cloud->empty()) {

      for (int h=0; h<result.rows; h++) {
        for (int w=0; w<result.cols; w++) {
            
            // Check if in bounding window
            if ( (h>y && h<(y+height)) && ((w > x) && w < (x+width)) ){
            
              // do nothing

            } else {

              // remove point
              //PointT point = cloud->at(w, h);
              //cloud->at(w, h);
              cloud->at(w, h).x = bad_point;
              cloud->at(w, h).y = bad_point;
              cloud->at(w, h).z = bad_point;
              
              cloud->at(w, h).r = bad_point;
              cloud->at(w, h).g = bad_point;
              cloud->at(w, h).b = bad_point;
            }
        }
      }
    }
  }
}

//.........这里部分代码省略.........
    std::cout <<"You can disable the transform with the --NT flag\n";    
  }
  
  pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution,!disable_transform);
  super.setInputCloud (cloud);
  if (has_normals)
    super.setNormalCloud (input_normals);
  super.setColorImportance (color_importance);
  super.setSpatialImportance (spatial_importance);
  super.setNormalImportance (normal_importance);
  std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
 
  std::cout << "Extracting supervoxels!\n";
  super.extract (supervoxel_clusters);
  std::cout << "Found " << supervoxel_clusters.size () << " Supervoxels!\n";
  PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
  PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
  PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
  PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud ();
  
  std::cout << "Getting supervoxel adjacency\n";
  std::multimap<uint32_t, uint32_t> label_adjacency;
  super.getSupervoxelAdjacency (label_adjacency);
   
  std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > refined_supervoxel_clusters;
  std::cout << "Refining supervoxels \n";
  super.refineSupervoxels (3, refined_supervoxel_clusters);

  PointLCloudT::Ptr refined_labeled_voxel_cloud = super.getLabeledVoxelCloud ();
  PointNCloudT::Ptr refined_sv_normal_cloud = super.makeSupervoxelNormalCloud (refined_supervoxel_clusters);
  PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud ();
  
  // THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS
  if (cloud->isOrganized ())
  {
    pcl::io::savePNGFile (out_label_path, *full_labeled_cloud, "label");
    pcl::io::savePNGFile (refined_out_label_path, *refined_full_labeled_cloud, "label");
    //Save RGB from labels
    pcl::io::PointCloudImageExtractorFromLabelField<PointLT> pcie (pcie.io::PointCloudImageExtractorFromLabelField<PointLT>::COLORS_RGB_GLASBEY);
    //We need to set this to account for NAN points in the organized cloud
    pcie.setPaintNaNsWithBlack (true);
    pcl::PCLImage image;
    pcie.extract (*full_labeled_cloud, image);
    pcl::io::savePNGFile (out_path, image);
    pcie.extract (*refined_full_labeled_cloud, image);
    pcl::io::savePNGFile (refined_out_path, image);
  }
  
  std::cout << "Constructing Boost Graph Library Adjacency List...\n";
  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
  typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
  typedef VoxelAdjacencyList::edge_descriptor EdgeID;
  VoxelAdjacencyList supervoxel_adjacency_list;
  super.getSupervoxelAdjacencyList (supervoxel_adjacency_list);

  
  std::cout << "Loading visualization...\n";
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->registerKeyboardCallback(keyboard_callback, 0);

 
  bool refined_normal_shown = show_refined;
  bool refined_sv_normal_shown = show_refined;
  bool sv_added = false;
  bool normals_added = false;

int main (int argc, char** argv)
{
  ros::init(argc, argv, "cv_proj");
  //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_02_indoor.pcd";
  //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_03_outdoor.pcd";
  /*
  std::string input_file = "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd";
  std::string output_file = "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd";
  std::string template_file = "/home/igor/pcds/templates/indoor_template.pcd";
  std::string out_rgb = "/home/igor/pcds/cv_proj_out/out_result_05.jpg";
  */
  std::string input_file, out_pcd, template_file, out_rgb, out_transf_pcd;

  ros::param::param<std::string>("/cv_proj/input_file", input_file, "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd");
  //ros::param::param<std::string>("/cv_proj/out_pcd", out_pcd, "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd");
  ros::param::param<std::string>("/cv_proj/template_file", template_file, "/home/igor/pcds/templates/indoor_template.pcd");
  //ros::param::param<std::string>("/cv_proj/out_rgb", out_rgb, "/home/igor/pcds/cv_proj_out/out_result_05.jpg");
  


  boost::filesystem::path filepath(input_file);
  boost::filesystem::path filename = filepath.filename();
  filename = filename.stem(); // Get rid of the extension
  boost::filesystem::path dir = filepath.parent_path();

  std::string opencv_out_ext = "_filtered.png";
  std::string pcl_out_ext = "_filtered.pcd";
  std::string output_folder = "/output_cv_proj/";
  std::string output_stem;
  output_stem = dir.string() + output_folder + filename.string();

  out_rgb = output_stem + opencv_out_ext;
  out_pcd = output_stem + pcl_out_ext;
  out_transf_pcd = output_stem + "_templ" + pcl_out_ext;

  std::cout << out_rgb << std::endl;
  std::cout << out_pcd << std::endl;

  // Read in the cloud data
  pcl::PCDReader reader;
  PointCloudT::Ptr cloud (new PointCloudT), cloud_f (new PointCloudT);
  reader.read (input_file, *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*

  if (cloud->isOrganized()) {
    std::cout << "-- PointCloud cloud is organized" << std::endl;
    std::cout << "-- PointCloud cloud width: " << cloud->width << std::endl;
    std::cout << "-- PointCloud cloud height: " << cloud->height << std::endl;
  }

  /*
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<PointT> vg;
  PointCloudT::Ptr cloud_filtered (new PointCloudT);
  vg.setInputCloud (cloud);
  //vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.setLeafSize (0.001f, 0.001f, 0.001f);
  //
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*
  */
  /**/
  PointCloudT::Ptr cloud_filtered (new PointCloudT);
  *cloud_filtered = *cloud;



  // Create the segmentation object for the planar model and set all the parameters
  pcl::SACSegmentation<PointT> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  PointCloudT::Ptr cloud_plane (new PointCloudT ());
  pcl::PCDWriter writer;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.02);

  int i=0, nr_points = (int) cloud_filtered->points.size ();
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<PointT> extract;

    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    //extract.setUserFilterValue(999);
    extract.setKeepOrganized(true); 
//.........这里部分代码省略.........