C++ PCLVisualizer::addPointCloud方法代码示例

  /* \brief Visual update. Create visualizations and add them to the viewer
   *
   */
  void update()
  {
    //remove existing shapes from visualizer
    clearView();

    //prevent the display of too many cubes
    bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;

    showLegend(displayCubeLegend);

    if (displayCubeLegend)
    {
      //show octree as cubes
      showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
      if (showPointsWithCubes)
      {
        //add original cloud in visualizer
        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
        viz.addPointCloud(cloud, color_handler, "cloud");
      }
    }
    else
    {
      //add current cloud in visualizer
      pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
      viz.addPointCloud(displayCloud, color_handler, "cloud");
    }
  }

 void addVoxels(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
 {
   pcl::PointCloud< pcl::PointXYZRGBA>::Ptr colored_voxel_cloud = supervoxels->getColoredVoxelCloud();
   visualizer.addPointCloud(colored_voxel_cloud, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, name);
 }

int main (int argc, char const* argv[])
{
	if (argc != 2) {
		cout << "Usage : obb_test filename.pcd" << endl;
		return 1;
	}
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 	if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud) == -1) {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return 1;
  }

	cloud_viewer.addPointCloud (cloud, "single_cloud");

	OrientedBoundingBox obb;
	Eigen::Quaternionf q;
	Eigen::Vector3f t, dims;
	obb.compute_obb_pca (cloud, q, t, dims);
  cloud_viewer.addCube(t, q, dims.x(), dims.y(), dims.z());
  cout << dims.x() << " " << dims.y() << " " << dims.z() << endl;
  
  while (!cloud_viewer.wasStopped()) {
    cloud_viewer.spinOnce(1);
	}
  
	return 0;
}

 bool
 drawParticles (pcl::visualization::PCLVisualizer& viz)
 {
   ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
   if (particles)
   {
     if (visualize_particles_)
     {
       pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
       for (size_t i = 0; i < particles->points.size (); i++)
       {
         pcl::PointXYZ point;
         
         point.x = particles->points[i].x;
         point.y = particles->points[i].y;
         point.z = particles->points[i].z;
         particle_cloud->points.push_back (point);
       }
       
       {
         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
         if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
           viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
       }
     }
     return true;
   }
   else
   {
     PCL_WARN ("no particles\n");
     return false;
   }
 }

 void addCentroids(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
 {
   //    pcl::PointCloud< pcl::PointXYZRGBA>::Ptr voxel_centroid_cloud = supervoxels->getVoxelCentroidCloud();
   visualizer.addPointCloud(supervoxelcloud, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
   visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.95, name);
 }

    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      if (!cloud_ || !new_cloud_)
      {
        boost::this_thread::sleep (boost::posix_time::milliseconds (1));
        return;
      }

      {
        boost::mutex::scoped_lock lock (mtx_);
        FPS_CALC ("visualization");
        CloudPtr temp_cloud;
        temp_cloud.swap (cloud_pass_);

        if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
        {
          viz.addPointCloud (temp_cloud, "OpenNICloud");
          viz.resetCameraViewpoint ("OpenNICloud");
        }
        // Render the data 
        if (new_cloud_ && cloud_hull_)
        {
          viz.removePointCloud ("hull");
          viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
        }
        new_cloud_ = false;
      }
    }

    void
    viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
      mtx_.lock ();
      if (!cloud_ || !normals_)
      {
        mtx_.unlock ();
        return;
      }

      CloudConstPtr temp_cloud;
      pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
      temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
      temp_normals.swap (normals_);
      mtx_.unlock ();

      if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
      {
        viz.addPointCloud (temp_cloud, "OpenNICloud");
        viz.resetCameraViewpoint ("OpenNICloud");
      }
      // Render the data
      if (new_cloud_)
      {
        viz.removePointCloud ("normalcloud");
        viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
        new_cloud_ = false;
      }
    }

//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
  ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
  if (particles && new_cloud_)
    {
      //Set pointCloud with particle's points
      pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      for (size_t i = 0; i < particles->points.size (); i++)
	{
	  pcl::PointXYZ point;
          
	  point.x = particles->points[i].x;
	  point.y = particles->points[i].y;
	  point.z = particles->points[i].z;
	  particle_cloud->points.push_back (point);
	}

      //Draw red particles 
      {
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);

	if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
	  viz.addPointCloud (particle_cloud, red_color, "particle cloud");
      }
      return true;
    }
  else
    {
      return false;
    }
}

//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
  boost::mutex::scoped_lock lock (mtx_);
    
  if (!cloud_pass_)
    {
      std::this_thread::sleep_for(1s);
      return;
   }

  //Draw downsampled point cloud from sensor    
  if (new_cloud_ && cloud_pass_downsampled_)
    {
      CloudPtr cloud_pass;
      cloud_pass = cloud_pass_downsampled_;
    
      if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
	{
	  viz.addPointCloud (cloud_pass, "cloudpass");
	  viz.resetCameraViewpoint ("cloudpass");
	}
      bool ret = drawParticles (viz);
      if (ret)
        drawResult (viz);
    }
  new_cloud_ = false;
}

    void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
    {
        _viewer.removeAllPointClouds(0);
        _viewer.addPointCloud(mapFrame.makeShared(), "map");
        _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
        _viewer.spinOnce();

    }

template <typename PointT> void
tviewer::PointCloudObject<PointT>::addDataToVisualizer (pcl::visualization::PCLVisualizer& v)
{
  v.addPointCloud (data_, name_);
  v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, name_);
  v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, visibility_, name_);
  if (use_fixed_color_ != 0)
  {
    float r, g, b;
    std::tie (r, g, b) = getRGBFromColor (color_);
    v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, name_);
  }
}

 void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, bool firstRun)
 {
   if(firstRun)
   {
     visualizer.addPointCloud(cloud, "cloud");
     visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
   }
   else
   {
     visualizer.updatePointCloud(cloud, "cloud");
     visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
   }
 }

    void viz_cb (pcl::visualization::PCLVisualizer& viz)
    {
//    cout << "PbMapMaker::viz_cb(...)\n";
      if (cloud1->empty())
      {
        boost::this_thread::sleep (boost::posix_time::milliseconds (10));
        return;
      }

      {
//        boost::mutex::scoped_lock lock (viz_mutex);

//        viz.removeAllShapes();
        viz.removeAllPointClouds();

        { //mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
          boost::mutex::scoped_lock updateLock(visualizationMutex);

//          if (!viz.updatePointCloud (cloud, "sphereCloud"))
//            viz.addPointCloud (sphereCloud, "sphereCloud");

          if (!viz.updatePointCloud (cloud1, "cloud1"))
            viz.addPointCloud (cloud1, "cloud1");

          if (!viz.updatePointCloud (cloud2, "cloud2"))
            viz.addPointCloud (cloud2, "cloud2");

          if (!viz.updatePointCloud (cloud3, "cloud3"))
            viz.addPointCloud (cloud3, "cloud3");

          if (!viz.updatePointCloud (cloud4, "cloud4"))
            viz.addPointCloud (cloud4, "cloud4");

          if (!viz.updatePointCloud (cloud5, "cloud5"))
            viz.addPointCloud (cloud5, "cloud5");

          if (!viz.updatePointCloud (cloud6, "cloud6"))
            viz.addPointCloud (cloud6, "cloud6");

          if (!viz.updatePointCloud (cloud7, "cloud7"))
            viz.addPointCloud (cloud7, "cloud7");

          if (!viz.updatePointCloud (cloud8, "cloud8"))
            viz.addPointCloud (cloud8, "cloud8");
        updateLock.unlock();
        }
      }
    }

 void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
 {
     double pointSize = 1.0;
     if(firstRun)
     {
         visualizer.addPointCloud(dispCloudPtr_, std::string("stuff"));
         visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
     }
     else
     {
         visualizer.updatePointCloud(dispCloudPtr_, std::string("stuff"));
         visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
     }
 }

  void filterAdjacency(pcl::visualization::PCLVisualizer &visualizer)
  {
    visualizer.removeAllPointClouds();
    visualizer.removeAllShapes();
    std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
    supervoxels->getSupervoxelAdjacency(supervoxel_adjacency);

    pcl::PointCloud< pcl::PointXYZRGBA>::Ptr filtered_supervoxel(new pcl::PointCloud< pcl::PointXYZRGBA>);
    for(std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); label_itr != supervoxel_adjacency.end();)
    {
      //First get the label
      uint32_t supervoxel_label = label_itr->first;
      //Now get the supervoxel corresponding to the label
      pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label);

      int threshold = 7 - supervoxel->centroid_.z * 0.375;

      int count = supervoxel_adjacency.count(supervoxel_label);
      if(count >= threshold)
      {
        //Now we need to iterate through the adjacent supervoxels and make a point cloud of them
        pcl::PointCloud< pcl::PointXYZRGBA>::Ptr adjacent_supervoxel_centers(new pcl::PointCloud< pcl::PointXYZRGBA>);

        std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first;
        for(; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr)
        {

          uint32_t snd_label = adjacent_itr->second;
          int snd_count = supervoxel_adjacency.count(snd_label);
          pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr neighbor_supervoxel = supervoxel_clusters.at(snd_label);
          int snd_threshold = 7 - neighbor_supervoxel->centroid_.z * 0.375;
          if(snd_count >= snd_threshold)
          {
            adjacent_supervoxel_centers->push_back(neighbor_supervoxel->centroid_);
            filtered_supervoxel->push_back(neighbor_supervoxel->centroid_);
          }
        }
        //Now we make a name for this polygon
        std::stringstream ss;
        ss << "supervoxel_" << supervoxel_label;
        //This function generates a "star" polygon mesh from the points given
        addSupervoxelConnectionsToViewer(supervoxel->centroid_, *adjacent_supervoxel_centers, ss.str(), visualizer);

      }
      label_itr = supervoxel_adjacency.upper_bound(supervoxel_label);
    }
    const std::string foo = "fassdfasdf";
    visualizer.addPointCloud(filtered_supervoxel, foo);
    visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, foo);
  }