#include <pcl/console/parse.h>   //命令行解析头文件

#include <pcl/point_types.h>     //点云数据类型

#include <pcl/visualization/pcl_visualizer.h>   //可视化头文件

#include <pcl/io/openni_grabber.h>                //kinect抓取的头文件

#include <pcl/sample_consensus/sac_model_plane.h>   //sac_model_plane随机采样一致形平面的模板

#include <pcl/people/ground_based_people_detection_app.h>

#include <pcl/common/time.h>

typedef pcl::PointXYZRGBA PointT;

typedef pcl::PointCloud<PointT> PointCloudT;

// PCL viewer //

pcl::visualization::PCLVisualizer viewer("PCL Viewer");  //可视化

// Mutex: //

boost::mutex cloud_mutex;

enum { COLS = , ROWS =  };    //窗口大小

int print_help()

{

  cout << "*******************************************************" << std::endl;

  cout << "Ground based people detection app options:" << std::endl;

  cout << "   --help    <show_this_help>" << std::endl;

  cout << "   --svm     <path_to_svm_file>" << std::endl;

  cout << "   --conf    <minimum_HOG_confidence (default = -1.5)>" << std::endl;

  cout << "   --min_h   <minimum_person_height (default = 1.3)>" << std::endl;

  cout << "   --max_h   <maximum_person_height (default = 2.3)>" << std::endl;

  cout << "   --sample  <sampling_factor (default = 1)>" << std::endl;

  cout << "*******************************************************" << std::endl;

  return ;

}

void cloud_cb_ (const PointCloudT::ConstPtr &callback_cloud, PointCloudT::Ptr& cloud,

    bool* new_cloud_available_flag)

{

  cloud_mutex.lock ();    // for not overwriting the point cloud from another thread锁住进程防止从其他进程写入点云

  *cloud = *callback_cloud;    //形参点云赋值

  *new_cloud_available_flag = true;    //标志位赋值为真

  cloud_mutex.unlock ();      //解锁进程

}

    //申明结构体用于传递参数给回调函数

struct callback_args{

  // structure used to pass arguments to the callback function

  PointCloudT::Ptr clicked_points_3d;                  //按键三次

  pcl::visualization::PCLVisualizer::Ptr viewerPtr;    //可视化

};

void

pp_callback (const pcl::visualization::PointPickingEvent& event, void* args)

{

  struct callback_args* data = (struct callback_args *)args;

  if (event.getPointIndex () == -)

    return;

  PointT current_point;

  event.getPoint(current_point.x, current_point.y, current_point.z);

  data->clicked_points_3d->points.push_back(current_point);

  // Draw clicked points in red:

  pcl::visualization::PointCloudColorHandlerCustom<PointT> red (data->clicked_points_3d, , , );

  data->viewerPtr->removePointCloud("clicked_points");

  data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");

  data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, , "clicked_points");

  std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;

}

int main (int argc, char** argv)

{

  if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))

        return print_help();

  // Algorithm parameters:算法参数

  std::string svm_filename = "/home/salm/myopencv/yl_pcl/people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";

  float min_confidence = -1.5;

  float min_width = 0.1;

  float max_width = 8.0;

  float min_height = 1.3;

  float max_height = 2.3;

  float voxel_size = 0.06;    //体素大小

  float sampling_factor = ;    //采样因子

  Eigen::Matrix3f rgb_intrinsics_matrix;

  rgb_intrinsics_matrix << , 0.0, 319.5, 0.0, , 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics相机的内参

  // Read if some parameters are passed from command line:

  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);

  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);

  pcl::console::parse_argument (argc, argv, "--min_h", min_height);

  pcl::console::parse_argument (argc, argv, "--max_h", max_height);

  pcl::console::parse_argument (argc, argv, "--sample", sampling_factor);

  // Read Kinect live stream:  读取kienct的数据流

  PointCloudT::Ptr cloud (new PointCloudT);

  bool new_cloud_available_flag = false;

  pcl::Grabber* interface = new pcl::OpenNIGrabber();

  boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =

      boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);

  interface->registerCallback (f);

  interface->start ();

  // Wait for the first frame:等待第一帧数据流

  while(!new_cloud_available_flag)

    boost::this_thread::sleep(boost::posix_time::milliseconds());

  new_cloud_available_flag = false;

  cloud_mutex.lock ();    // for not overwriting the point cloud锁住进程

  // Display pointcloud:显示点云

  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);

  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");

  viewer.setCameraPosition(,,-,,-,,);  //设置相机的位置

  // Add point picking callback to viewer:

  struct callback_args cb_args;

  PointCloudT::Ptr clicked_points_3d (new PointCloudT);

  cb_args.clicked_points_3d = clicked_points_3d;

  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);

  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);

  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:

  viewer.spin();

  std::cout << "done." << std::endl;

  cloud_mutex.unlock ();    

  // Ground plane estimation:

  Eigen::VectorXf ground_coeffs;

  ground_coeffs.resize();

  std::vector<int> clicked_points_indices;

  for (unsigned int i = ; i < clicked_points_3d->points.size(); i++)

    clicked_points_indices.push_back(i);

  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);

  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);

  std::cout << "Ground plane: " << ground_coeffs() << " " << ground_coeffs() << " " << ground_coeffs() << " " << ground_coeffs() << std::endl;

  // Initialize new viewer:可视化初始化

  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization

  viewer.setCameraPosition(,,-,,-,,);

  // Create classifier for people detection:创建分类器

  pcl::people::PersonClassifier<pcl::RGB> person_classifier;

  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:  检测初始化

  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object

  people_detector.setVoxelSize(voxel_size);                        // set the voxel size

  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters

  people_detector.setClassifier(person_classifier);                // set person classifier

  //people_detector.setPersonClusterLimits(min_height, max_height, min_width, max_width);

  people_detector.setSamplingFactor(sampling_factor);              // set a downsampling factor to the point cloud (for increasing speed)

//  people_detector.setSensorPortraitOrientation(true);              // set sensor orientation to vertical

  // For timing:

  static unsigned count = ;

  static double last = pcl::getTime ();

  // Main loop:

  while (!viewer.wasStopped())

  {

    if (new_cloud_available_flag && cloud_mutex.try_lock ())    // if a new cloud is available

    {

      new_cloud_available_flag = false;

      // Perform people detection on the new cloud:显示检测到的新点云

      std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters

      people_detector.setInputCloud(cloud);

      people_detector.setGround(ground_coeffs);                    // set floor coefficients

      people_detector.compute(clusters);                           // perform people detection

      ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

      // Draw cloud and people bounding boxes in the viewer:

      viewer.removeAllPointClouds();

      viewer.removeAllShapes();

      pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);

      viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");

      unsigned int k = ;

      for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)

      {

        if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold

        {

          // draw theoretical person bounding box in the PCL viewer:

          it->drawTBoundingBox(viewer, k);

          k++;

        }

      }

      std::cout << k << " people found" << std::endl;

      viewer.spinOnce();

      // Display average framerate:

      if (++count == )

      {

        double now = pcl::getTime ();

        std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;

        count = ;

        last = now;

      }

      cloud_mutex.unlock ();

    }

  }