代码分享：基于PCL获取Kinect1数据+可视化+突出某个区域+一键保存点云数据

背景：

基于PCL获取Kinect1的实时点云，并可视化，这个功能在官方的指导教程点击打开链接里有。但是，我需要突出视野中心的那部分点云，还有加一个一键保存点云数据的功能。所以我自己写了个小程序，代码在下面有。

注：

还有，官方的指导教程里的代码会使Kinect读到的点云在可视化的时候是上下颠倒的，我修复了这个bug。

如果跑我的代码时候，遇到这个问题error: pcl/io/openni_grabber.h: No such file or directory，请参考我的另一文章：点击打开链接

1. 代码：

#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <string>
#include <iostream>
#include <vector>

boost::mutex cloud_mutex;
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_global;

class SimpleOpenNIViewer
{
  public:
    SimpleOpenNIViewer ()   {}

    //回调函数，回传收到的点云到run()函数里
    void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out, bool* new_cloud_available_flag)
    {
        //锁住，不允许此段时间内，点云数据被修改
        cloud_mutex.lock();
        *cloud_out = *cloud;
        *new_cloud_available_flag = true;
        cloud_mutex.unlock();
    }
    //kinect读到的点云是在viewer.showCloud里是颠倒的，这个函数就是把它再颠倒过
    pcl::PointCloud<pcl::PointXYZ>::Ptr upsideDown(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in){
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
        transform(1,1) = -1;
        transform(2,2) = -1;
        pcl::transformPointCloud(*cloud_in, *cloud_transformed, transform);
        return cloud_transformed;
    }
    //突出中间的部分的点云
    pcl::PointCloud<pcl::PointXYZI>::Ptr highlightMiddleArea(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in){

        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PassThrough<pcl::PointXYZ> pt;
        pcl::IndicesPtr reserve_indices (new std::vector <int>);

        pcl::copyPointCloud(*cloud_in, *cloud_out);
        pt.setInputCloud(cloud_in);
        pt.setFilterFieldName("x");
        //突出x方向上（-0.3,0.3）这个范围内的点云
        pt.setFilterLimits(-0.3, 0.3);
        pt.filter(*reserve_indices);
        for (int i = 0; i < (*reserve_indices).size(); i++){
            cloud_out->points[(*reserve_indices)[i]].intensity = 10;
        }
        return cloud_out;
    }
    //代码的核心部分，整个流程都在这里安排
    void run (boost::shared_ptr<pcl::visualization::CloudViewer> viewer)
    {
      bool new_cloud_available_flag = false;
      pcl::Grabber* interface = new pcl::OpenNIGrabber();
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_toshow (new pcl::PointCloud<pcl::PointXYZI>);

      //注册回调函数
      boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
        boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1, cloud_out, &new_cloud_available_flag);
      interface->registerCallback (f);
      //start()之后，点云数据源源不断的传到cloud_out
      interface->start ();

      while (!viewer->wasStopped())
      {
          if (new_cloud_available_flag){//如果有新的点云获取
              cloud_mutex.lock();       //锁住，不允许此段时间内，点云数据被修改
              cloud_tmp = upsideDown(cloud_out);//颠倒点云，为了可视化
              cloud_mutex.unlock();
              cloud_toshow = highlightMiddleArea(cloud_tmp);
              viewer->showCloud(cloud_toshow);//可视化点云
              cloud_global = cloud_tmp;       //传点云到keyboardEventOccured()里
              new_cloud_available_flag = false;
          }
      }
      interface->stop ();
    }
};
//回调函数，当键盘有输入时，被调用
void keyboardEventOccured(const pcl::visualization::KeyboardEvent &event, void *nothing){
    if (event.getKeySym() == "space" && event.keyDown()){//当按下空格键时
        cout << "Space is pressed => pointcloud saved as output.pcd" << endl;
        pcl::io::savePCDFile("output.pcd", *cloud_global);
    }
}
int main ()
{
  boost::shared_ptr<pcl::visualization::CloudViewer> viewer2 (new pcl::visualization::CloudViewer ("a viewer"));
  //绑定可视化窗口和键盘事件的函数
  viewer2->registerKeyboardCallback(keyboardEventOccured,(void*)NULL);

  SimpleOpenNIViewer v;
  v.run (viewer2);
  return 0;
}

2. 跑的效果

GetDataFromKinect/bin/GetKinectData...

Space is pressed => pointcloud saved as output.pcd