PCL库实时显示点云流

近期需要将三维点云图实时显示出来，关于点云库，有PCL和opengl。pcl在处理点云的算法上有优势，opengl做点云的显示与渲染有优势。

由于点云处理操作较多，所以就选择了PCL的库来处理。

PCL中点云的显示主要有两个类：1. pcl::visualization::CloudViewer; 2. pcl::visualization::PCLVisualizer。

前面一个类主要做简单的点云显示，后面一个有更加丰富的设置接口。下面简单的介绍两种

1. pcl::visualization::CloudViewer（这里偷懒直接把官方的示例拖过来，网址：http://www.pointclouds.org/documentation/tutorials/cloud_viewer.php#cloud-viewer）

•           下面是最简单的显示，其中PointXYZRGB可以替换为PointXYZ, PointXYZRGBA等多种点云的格式。

#include <pcl/visualization/cloud_viewer.h>
//...
void
foo ()
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
  //... populate cloud
  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
  viewer.showCloud (cloud);
  while (!viewer.wasStopped ())
  {
  }

}

•       如果想要在一个单独的线程中跑，可以看下面这个例子

#include <pcl/visualization/cloud_viewer.h>#include <iostream>#include <pcl/io/io.h>#include <pcl/io/pcd_io.h>
   
int user_data;
   
void
viewerOneOff (pcl::visualization::PCLVisualizer& viewer){
    viewer.setBackgroundColor (1.0, 0.5, 1.0);
    pcl::PointXYZ o;
    o.x = 1.0;
    o.y = 0;
    o.z = 0;
    viewer.addSphere (o, 0.25, "sphere", 0);
    std::cout << "i only run once" << std::endl;
   
}
   
void
viewerPsycho (pcl::visualization::PCLVisualizer& viewer){
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 200, 300, "text", 0);
   
    //FIXME: possible race condition here:
    user_data++;}
   
int
main (){
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::io::loadPCDFile ("my_point_cloud.pcd", *cloud);
   
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
   
    //blocks until the cloud is actually rendered
    viewer.showCloud(cloud);
   
    //use the following functions to get access to the underlying more advanced/powerful
    //PCLVisualizer
   
    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);
   
    //This will get called once per visualization iteration
    viewer.runOnVisualizationThread (viewerPsycho);
    while (!viewer.wasStopped ())
    {
    //you can also do cool processing here
    //FIXME: Note that this is running in a separate thread from viewerPsycho
    //and you should guard against race conditions yourself...
    user_data++;
    }

    return 0;}

2.  如果想要更加丰富的点云显示，如给点云添加颜色、显示点云中的法矢、在窗口中自己画图案、自定义视角的位置，可以采用pcl::visualization::PCLVisualizer。

           在官方网页 ：http://www.pointclouds.org/documentation/tutorials/pcl_visualizer.php#pcl-visualizer  有详细的点云显示示例代码，这里不再粘贴。

           下面贴出一个基于pcl::visualization::PCLVisualizer 实时显示点云流的代码。

#include <pcl/point_types.h>

#include <pcl/visualization/cloud_viewer.h>

boost::mutex updateModelMutex;

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)

{

        // --------------------------------------------

        // -----Open 3D viewer and add point cloud-----

        // --------------------------------------------

        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

        viewer->setCameraPosition(0, 0, 0, 0, 0, 0, 0, 0, -1);

        viewer->setBackgroundColor(0,0,0);

        viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");

        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

        viewer->initCameraParameters ();

        return (viewer);

}

void viewerRunner(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer)

{

        while (!viewer->wasStopped ())

        {

               viewer->spinOnce (100);

               boost::this_thread::sleep (boost::posix_time::microseconds (100));

        }

}

void main()

{      

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);

        pcl::PointCloud<pcl::PointXYZ> &pcloud1 = *cloud_ptr;

        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

        

        pcloud1.points.push_back( pcl::PointXYZ(10, 10, 80) );

        pcloud1.width = cloud_ptr->size();

        pcloud1.height = 1;

        pcloud1.is_dense = true;

        viewer = simpleVis(cloud_ptr);

        boost::thread vthread(&viewerRunner,viewer);

       

        while(1)//循环抓取深度数据

        {

              pcloud1.clear();

              for ( int _row = 0; _row < disp.rows; _row++ )

              {

                   for ( int _col = 0; _col < disp.cols; _col++ )

                   {

                       float x, y, z;

                       pcl::PointXYZ ptemp(x, y, z);

                       pcloud1.points.push_back( ptemp );

                    }

               }

               pcloud1.width = cloud_ptr->size();

               pcloud1.height = 1;

               pcloud1.is_dense = true;

               boost::mutex::scoped_lock updateLock(updateModelMutex);

               viewer->updatePointCloud<pcl::PointXYZ>(cloud_ptr,"sample cloud");

               updateLock.unlock();

               boost::this_thread::sleep (boost::posix_time::microseconds (100));

       }

       vthread.joint();

}