Gazebo下的双臂协作机器人的工件装配（三）基于点云和yolo

安装PCL

PCL（Point Cloud Library）是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。

具体安装过程不在赘述，大致安装过程如下：

1.安装各种依赖库

2.Qt4切换到Qt5

1
2
3

qmake -v
cd /usr/lib/x86_64-linux-gnu/qt-default/qtchooser
sudo gedit default.conf

3.通过编译源码安装VTK7.1.1，需要cmake gui

4.推荐直接通过命令行安装，通过源码安装依赖太多编译不容易通过

（通过命令行安装的PCL版本是1.7.2）

安装好后可以找个例程测试一下pcl visualizer能否正常显示

相关例程及cmakelist百度云:https://pan.baidu.com/s/1hZU0FzA3iyXTCS-AHXWS1Q

搭建环境

该world在原来食品分类的基础上修改得到

工件STL模型百度云：https://pan.baidu.com/s/13mkX0bAiHVC86QqyWYthbQ

由于相机畸变等因素，对于较长的柱状工件，其侧面在点云中或是RGB图像中对位姿估计的干扰都太大，因此为了简化任务量，索性将任务改为将正方体工件装配到固定位置的柱状工件上面

位姿估计

位姿估计采用点云进行欧几里得聚类和RANSAC拟合直线的方法；
至于正方体的分类并没有采用RGB图像进行边缘检测和多边形逼近，而是采用了深度神经网络进行分类再将分类的预测框中心和各个聚类的中心进行匹配从而得出各个聚类对应的工件名称

点云处理

创建功能包

1 2	catkin_create_pkg pclproc pcl_conversions pcl_ros pcl_msgs roscpp rospy std_msgs darknet_ros_msgs sensor_msgs catkin_make -DCATKIN_WHITELIST_PACKAGES="pclproc"

1.格式转换

由 sensor_msgs::PCLPointCloud2转换为 pcl::PointCloud<pcl::PointXYZ>

具体方法方法可参考（PCL中点云数据格式之间的转化 - Being_young - 博客园 (cnblogs.com)）

void pcl::toROSMsg(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &);

2.直通滤波

对于在空间分布有一定空间特征的点云数据，比如使用线结构光扫描的方式采集点云，沿z向分布较广，但x,y向的分布处于有限范围内。此时可使用直通滤波器，确定点云在x或y方向上的范围，可较快剪除离群点，达到第一步粗处理的目的。

cout << " Cloud before filtering: " << cloud->points.size() << endl;
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");//设置过滤时所需要的点云Z字段
pass.setFilterLimits(0.49, 0.52);//设置在过滤字段上的范围
pass.setNegative(false);//设置保留范围内的还是过滤掉范围内的
pass.filter(*cloudf);
cout << "Cloud after filtering: " << cloudf->points.size() << endl;

3.体素栅格下采样

由于点云的海量和无序性，直接处理的方式在对邻域进行搜索时需要较高的计算成本。一个常用的解决方式就是对点云进行下采样，将对全部点云的操作转换到下采样所得到的点上，降低计算量；所谓体素栅格是将点云空间划分为一个个极小的格子，格子里包含几个点。对体素栅格里面的点取平均或者加权平均，得到一个点，代替原来的几个点。显然，栅格选的越大，滤波后点云点数越少，速度快，但是会对原来的点云过度模糊;栅格选的越小，作用反之。

std::cout << "PointCloud before sampling has: " << cloudf->points.size () << " data points." << std::endl; //*
/*从输入的.PCD文件载入数据后，我们创建了一个VoxelGrid滤波器对数据进行下采样，我们在这里进行下采样的原因是来加速处理过程，越少的点意味着分割循环中处理起来越快。*/
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg; //体素栅格下采样对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloudf);
vg.setLeafSize (0.005f, 0.005f, 0.005f); //设置采样的体素大小
vg.filter (*cloud_filtered);  //执行采样保存数据
std::cout << "PointCloud after sampling has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

左图为直通滤波，右图为下采样

4.欧几里得聚类

//欧几里德聚类
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered); //创建点云索引向量，用于存储实际的点云信息
  pcl::PCDWriter writer;
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.015); //设置近邻搜索的搜索半径为2cm
  ec.setMinClusterSize (100);//设置一个聚类需要的最少点数目为100
  ec.setMaxClusterSize (25000);//设置一个聚类需要的最大点数目为25000
  ec.setSearchMethod (tree);//设置点云的搜索机制
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);//从点云中提取聚类，并将点云索引保存在cluster_indices中

  /*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*/
  //迭代访问点云索引cluster_indices，直到分割出所有聚类
  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
     cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    std::stringstream ss;
    ss << "cloud_cluster_" << j << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
  }

5.边缘提取与质心估计

对点云数据集的每个点的法线估计，可以看作是对表面法线的近似推断，从原始点云上计算出法线，再由法线结合数据估计出边界。

//对各个聚类提取边缘
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Boundary> boundaries;
    pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());

    //pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;  //其中pcl::PointXYZ表示输入类型数据，pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst;
    normEst.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误
    normEst.setInputCloud(cloud_cluster);
    normEst.setSearchMethod(tree2);
    normEst.setRadiusSearch(0.02);  //法向估计的半径
     //normEst.setKSearch(9);  //法向估计的点数
    normEst.compute(*normals);
    //cout << "normal size is " << normals->size() << endl;
    est.setInputCloud(cloud_cluster);
    est.setInputNormals(normals);
    est.setSearchMethod(tree2);
    est.setKSearch(500);  //一般这里的数值越高，最终边界识别的精度越好
    est.compute(boundaries);
    pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> noBoundPoints;
    int countBoundaries = 0;
    for (int i = 0; i<cloud_cluster->size(); i++) 
    {
      uint8_t x = (boundaries.points[i].boundary_point);
      int a = static_cast<int>(x); //该函数的功能是强制类型转换
      if (a == 1)
      {
        (*boundPoints).push_back(cloud_cluster->points[i]);
        countBoundaries++;
      }
      else
        noBoundPoints.push_back(cloud_cluster->points[i]);
    }
    std::cout << "boundary "<<j<<" size is：" << countBoundaries << std::endl;

质心估计

1 2	Eigen::Vector4f centroid; pcl::compute3DCentroid(*boundPoints, centroid);

得到的centroid[]是一个长度为3的数组

6.RANSAC拟合边缘直线

RANSAC(RAndom SAmple Consensus,随机采样一致)算法是从一组含有“外点”(outliers)的数据中正确估计数学模型参数的迭代算法，一直迭代到估计出认为比较好的模型。
具体的实现步骤可以分为以下几步：

选择出可以估计出模型的最小数据集；(对于直线拟合来说就是两个点，对于计算Homography矩阵就是4个点)
使用这个数据集来计算出数据模型；
将所有数据带入这个模型，计算出“内点”的数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
比较当前模型和之前推出的最好的模型的“内点“的数量，记录最大“内点”数的模型参数和“内点”数；
重复1-4步，直到迭代结束或者当前模型已经足够好了(“内点数目大于一定数量”)。

//对边缘的四条边分别通过RANSAC进行直线拟合

   double list_dir[2];
   double list_pre_dir[4];
   Eigen::Vector4f centroid;
   pcl::compute3DCentroid(*boundPoints, centroid);   
   // std::cout << "点云 "<<j<<" 质心是（"
   // << 0.4-centroid[1] << ","
   // << -centroid[0] << ","
   // << centroid[2] << ")." << std::endl; 


   //创建一个模型参数对象，用于记录结果
   pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
   pcl::PointIndices::Ptr inliers(new pcl::PointIndices);  //inliers表示误差能容忍的点 记录的是点云的序号
   pcl::SACSegmentation<pcl::PointXYZ> seg2;     // 创建一个分割器
   seg2.setOptimizeCoefficients(true);      // Optional，这个设置可以选定结果平面展示的点是分割掉的点还是分割剩下的点。
   seg2.setModelType(pcl::SACMODEL_LINE);  // Mandatory-设置目标几何形状
   seg2.setMethodType(pcl::SAC_RANSAC);     //分割方法：随机采样法
   seg2.setDistanceThreshold(0.001);         //设置误差容忍范围，也就是阈值

   
   
   for(int i=0;i<4;i++)
   {    
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out_line(new pcl::PointCloud<pcl::PointXYZ>);
       seg2.setInputCloud(boundPoints);               //输入点云
       seg2.segment(*inliers, *coefficients);   //分割点云，获得平面和法向量
       //打印直线方程
       //std::cout << "d：" << coefficients->values[3] << endl;
       //std::cout << "e：" << coefficients->values[4] << endl;
       list_pre_dir[i]=coefficients->values[4]/coefficients->values[3];
       //std::cout << list_pre_dir[i]<<endl;
       pcl::ExtractIndices<pcl::PointXYZ> extract;   //创建点云提取对象
       extract.setInputCloud (boundPoints);    //设置输入点云
       extract.setIndices (inliers);   //设置分割后的内点为需要提取的点集
       extract.setNegative (false); //设置提取内点而非外点
       extract.filter (*cloud_line);   //提取输出存储到cloud_line
       std::cout << "PointCloud"<<i<< "representing the line component: " << cloud_line->points.size () << " data points." << std::endl;
       // Remove the planar inliers, extract the rest
       extract.setNegative (true);
       extract.filter (*cloud_out_line);
       *boundPoints = *cloud_out_line;
   }

由于正方形边缘具有四条边，每拟合出一条边余下的点便继续用于直线拟合，最后会拟合出四条直线，根据直线的前两个方向向量之比估算出直线斜率，四条直线斜率符号相同的两两之和取平均，便有了工件坐标系的x,y轴分量

7.工件分类

对于分类问题，尝试过1.用聚类中的点云数目比较大小，2.用上表面的高度比较大小—-但是效果都不太好
3.决定采用yolo进行分类，然后将预测框与之前点云估计出的质心相匹配，为什么这一步是跟pointcloud结合而不是rgb图像？1.正方体侧面干扰太大2.正方体是规则几何体，主方向容易取到对角线

采用yolo-v3的ros版本—darknet_ros,实现视频流的检测与分类

数据集百度云：链接：https://pan.baidu.com/s/11uRc4wOMqCsnLiakXJDeqg
复制这段内容后打开百度网盘手机App，操作更方便哦—来自百度网盘超级会员V1的分享

权重文件百度云：链接：https://pan.baidu.com/s/1mz-b-k-9gyn-ViGrn6yI6g
提取码：1234
复制这段内容后打开百度网盘手机App，操作更方便哦—来自百度网盘超级会员V1的分享

网络结构解析：

Yolov3中，只有卷积层，通过调节卷积步长控制输出特征图的尺寸。所以对于输入图片尺寸没有特别限制。流程图中，输入图片以256*256作为样例。
Yolov3借鉴了金字塔特征图思想，小尺寸特征图用于检测大尺寸物体，而大尺寸特征图检测小尺寸物体。特征图的输出维度为 $N\times N \times [3 \times (4 + 1 + 80)]$ ， $N\times N$ 为输出特征图格点数，一共3个Anchor框，每个框有4维预测框数值 $t_x ,t_y ,t_w, t_h$ ，1维预测框置信度，80维物体类别数。所以第一层特征图的输出维度为 $8 \times 8 \times 255$ 。
Yolov3总共输出3个特征图，第一个特征图下采样32倍，第二个特征图下采样16倍，第三个下采样8倍。输入图像经过Darknet-53（无全连接层），再经过Yoloblock生成的特征图被当作两用，第一用为经过33卷积层、11卷积之后生成特征图一，第二用为经过1*1卷积层加上采样层，与Darnet-53网络的中间层输出结果进行拼接，产生特征图二。同样的循环之后产生特征图三。
concat操作与加和操作的区别：加和操作来源于ResNet思想，将输入的特征图，与输出特征图对应维度进行相加，即 $y = f(x)+x$ ；而concat操作源于DenseNet网络的设计思路，将特征图按照通道维度直接进行拼接，例如8816的特征图与8816的特征图拼接后生成8832的特征图。
上采样层(upsample)：作用是将小尺寸特征图通过插值等方法，生成大尺寸图像。例如使用最近邻插值算法，将88的图像变换为1616。上采样层不改变特征图的通道数。

Yolo的整个网络，吸取了Resnet、Densenet、FPN的精髓，可以说是融合了目标检测当前业界最有效的全部技巧。

8.yolo预测框与各个点云聚类匹配

思路就是计算两者质心的距离，根据最小距离匹配出对应的预测框和点云

完整源码

#include <vector>
#include <ctime>
#include <iostream>
#include <string>
#include<math.h>
#include <ros/ros.h>
#include <Eigen/Core>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/passthrough.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h>
#include <pcl/features/eigen.h>
#include <pcl/features/feature.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/features/boundary.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/centroid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <darknet_ros_msgs/BoundingBoxes.h>
#include <darknet_ros_msgs/ObjectCount.h>


using namespace std;
using namespace sensor_msgs;
using namespace darknet_ros_msgs;
using namespace message_filters;
int objectnum;
struct Bounding
{
  string obj;
  int center_x;
  int center_y;
};
struct Grasp
{
  string name;
  double x;
  double y;
  double angle;
};
vector<Bounding>objects;
vector<Grasp>grasps;
void BubbleSort(double arr[], int n)
{
    for (int i = 0; i < n - 1; i++) {
        for (int j = 0; j < n - i - 1; j++) {
            if (arr[j] > arr[j + 1]) {
                double temp = arr[j];
                arr[j] = arr[j + 1];
                arr[j + 1] = temp;
            }
        }
    }
}

void msgtrans(const BoundingBoxes::ConstPtr &data)
{
  //由于这个vector是全局变量，每次使用前要清空
  objects.clear();
  Bounding b;
  for(int i=0;i<objectnum;i++)
  {
  	//此处作用：将订阅消息中的类别名和预测框中心的像素坐标存入vector
    b.obj=data->bounding_boxes[i].Class;
    b.center_x=round(((data->bounding_boxes[i].xmin)+(data->bounding_boxes[i].xmax))/2);
    b.center_y=round(((data->bounding_boxes[i].ymin)+(data->bounding_boxes[i].ymax))/2);
    objects.push_back(b);
  }
}
void yolomatchpcl(vector<Bounding>&a,vector<vector<double> >&b)
{
  grasps.clear();
  // for(int i=0;i<a.size();i++)
  int i,distance,minid=0;
  int mindistance=10000;
  Grasp g;
  while(grasps.size()<a.size())
  {
    for(int j=0;j<a.size();j++)
    {
    distance=(a[i].center_x-b[j][0])*(a[i].center_x-b[j][0])+(a[i].center_y-b[j][1])*(a[i].center_y-b[j][1]);
    if(distance<=mindistance)
      {
        mindistance=distance;
        minid=j;
      }
    }
    cout<<mindistance<<endl;
    g.name=a[i].obj;
    g.x=b[minid][0];
    g.y=b[minid][1];
    g.angle=b[minid][2];
    grasps.push_back(g);
    minid=0;
    mindistance=10000;
    ++i;
  }
  cout<<"匹配完成"<<endl;
}
int xtopixel(double x)
{
  //画面的y轴对应世界坐标系的x轴，所以需要颠倒过来，此处作用相当于手眼标定
  int ypixel=round((0.4-x)*480/0.475+240);
  return ypixel;
}
int ytopixel(double y)
{
  //画面的x轴对应世界坐标系的y轴，所以需要颠倒过来，此处作用相当于手眼标定
  int xpixel=round(-y*640/0.633+320);
  return xpixel;
}
void callback(const PointCloud2::ConstPtr &msg1,
              const BoundingBoxes::ConstPtr &msg2,const ObjectCount::ConstPtr &msg3) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudf (new pcl::PointCloud<pcl::PointXYZ>);
  
  objectnum=msg3->count;
  // pcl::PointCloud<pcl::PointXYZ> origincloud;
  //cloud=origincloud.makeShared();
  // pcl::fromROSMsg(*msg1, origincloud);

  pcl::fromROSMsg(*msg1, *cloud);
  msgtrans(msg2);
  cout << " Cloud before filtering: " << cloud->points.size() << endl;
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("z");//设置过滤时所需要的点云Z字段
  pass.setFilterLimits(0.49, 0.52);//设置在过滤字段上的范围
  pass.setNegative(false);//设置保留范围内的还是过滤掉范围内的
  pass.filter(*cloudf);
  std::cout << "Cloud after filtering: " << cloudf->points.size() << endl;
  std::cout << "PointCloud before sampling has: " << cloudf->points.size () << " data points." << std::endl; //*
  /*从输入的.PCD文件载入数据后，我们创建了一个VoxelGrid滤波器对数据进行下采样，我们在这里进行下采样的原因是来加速处理过程，越少的点意味着分割循环中处理起来越快。*/
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg; //体素栅格下采样对象
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  vg.setInputCloud (cloudf);
  vg.setLeafSize (0.005f, 0.005f, 0.005f); //设置采样的体素大小
  vg.filter (*cloud_filtered);  //执行采样保存数据
  std::cout << "PointCloud after sampling has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*
  //欧几里德聚类
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered); //创建点云索引向量，用于存储实际的点云信息
  // pcl::PCDWriter writer;
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.015); //设置近邻搜索的搜索半径为2cm
  ec.setMinClusterSize (100);//设置一个聚类需要的最少点数目为100
  ec.setMaxClusterSize (25000);//设置一个聚类需要的最大点数目为25000
  ec.setSearchMethod (tree);//设置点云的搜索机制
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);//从点云中提取聚类，并将点云索引保存在cluster_indices中
  /*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*/
  //迭代访问点云索引cluster_indices，直到分割出所有聚类
  int j = 0;
  vector<vector<double> > out;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
     cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    // std::stringstream ss;
    // ss << "cloud_cluster_" << j << ".pcd";
    // writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*

    //对各个聚类提取边缘
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Boundary> boundaries;
    pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());

    //pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;  //其中pcl::PointXYZ表示输入类型数据，pcl::Normal表示输出类型,且pcl::Normal前三项是法向，最后一项是曲率
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normEst;
    normEst.setNumberOfThreads(12);  // 手动设置线程数，否则提示错误
    normEst.setInputCloud(cloud_cluster);
    normEst.setSearchMethod(tree2);
    normEst.setRadiusSearch(0.02);  //法向估计的半径
     //normEst.setKSearch(9);  //法向估计的点数
    normEst.compute(*normals);
    //cout << "normal size is " << normals->size() << endl;
    est.setInputCloud(cloud_cluster);
    est.setInputNormals(normals);
    est.setSearchMethod(tree2);
    est.setKSearch(500);  //一般这里的数值越高，最终边界识别的精度越好
    est.compute(boundaries);
    pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> noBoundPoints;
    int countBoundaries = 0;
    for (int i = 0; i<cloud_cluster->size(); i++) 
    {
      uint8_t x = (boundaries.points[i].boundary_point);
      int a = static_cast<int>(x); //该函数的功能是强制类型转换
      if (a == 1)
      {
        (*boundPoints).push_back(cloud_cluster->points[i]);
        countBoundaries++;
      }
      else
        noBoundPoints.push_back(cloud_cluster->points[i]);
    }
    std::cout << "boundary "<<j<<" size is：" << countBoundaries << std::endl;
    // std::stringstream ss2;
    // pcl::PCDWriter writer;
    // ss2 << "cloud_boundry"<<j<< ".pcd";
    // writer.write<pcl::PointXYZ> (ss2.str (), *boundPoints, false); //*
    //对边缘的四条边分别通过RANSAC进行直线拟合

    double list_dir[2];
    double list_pre_dir[4];
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*boundPoints, centroid);   
    // std::cout << "点云 "<<j<<" 质心是（"
    // << 0.4-centroid[1] << ","
    // << -centroid[0] << ","
    // << centroid[2] << ")." << std::endl; 


    //创建一个模型参数对象，用于记录结果
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);  //inliers表示误差能容忍的点 记录的是点云的序号
    pcl::SACSegmentation<pcl::PointXYZ> seg2;     // 创建一个分割器
    seg2.setOptimizeCoefficients(true);      // Optional，这个设置可以选定结果平面展示的点是分割掉的点还是分割剩下的点。
    seg2.setModelType(pcl::SACMODEL_LINE);  // Mandatory-设置目标几何形状
    seg2.setMethodType(pcl::SAC_RANSAC);     //分割方法：随机采样法
    seg2.setDistanceThreshold(0.001);         //设置误差容忍范围，也就是阈值

    
    
    for(int i=0;i<4;i++)
    {    
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out_line(new pcl::PointCloud<pcl::PointXYZ>);
        seg2.setInputCloud(boundPoints);               //输入点云
        seg2.segment(*inliers, *coefficients);   //分割点云，获得平面和法向量
        //打印直线方程
        //std::cout << "d：" << coefficients->values[3] << endl;
        //std::cout << "e：" << coefficients->values[4] << endl;
        list_pre_dir[i]=coefficients->values[4]/coefficients->values[3];
        //std::cout << list_pre_dir[i]<<endl;
        pcl::ExtractIndices<pcl::PointXYZ> extract;   //创建点云提取对象
        extract.setInputCloud (boundPoints);    //设置输入点云
        extract.setIndices (inliers);   //设置分割后的内点为需要提取的点集
        extract.setNegative (false); //设置提取内点而非外点
        extract.filter (*cloud_line);   //提取输出存储到cloud_line
        std::cout << "PointCloud"<<i<< "representing the line component: " << cloud_line->points.size () << " data points." << std::endl;
        // Remove the planar inliers, extract the rest
        extract.setNegative (true);
        extract.filter (*cloud_out_line);
        *boundPoints = *cloud_out_line;
    }
    BubbleSort(list_pre_dir,4);
    // std::cout << list_pre_dir[0]<<list_pre_dir[1]<<list_pre_dir[2]<<list_pre_dir[3]<<endl;
    list_dir[0]=(list_pre_dir[0]+list_pre_dir[1])/2;
    list_dir[1]=(list_pre_dir[2]+list_pre_dir[3])/2;
    double angle_y=1.57-atan(list_dir[1]);
    //std::cout << "x,y方向的斜率：" << list_dir[0]<<","<<list_dir[1]<< endl;
    std::cout << "Z轴旋转角为：" << angle_y<< endl;
    vector<double>out_element;
    out_element.push_back(ytopixel(-centroid[0]));
    out_element.push_back(xtopixel(0.4-centroid[1]));
    out_element.push_back(angle_y);
    out.push_back(out_element);
    cout <<out.size()<<endl;
    // cout << "点云 "<<j<<" 质心是（"
    // << out[j][0] << ","
    // << out[j][1] <<")" << endl; 
    // cout << "yolo "<<j<<" 质心是（"
    // << objects[j].center_x << ","
    // << objects[j].center_y <<")" << endl; 
    j++;
  }
  yolomatchpcl(objects,out);
  for(int k=0;k<out.size();k++)
  {
    cout <<grasps[k].name<<" 质心是（"
    << grasps[k].x << ","
    << grasps[k].y <<")"
    <<"旋转角为"<<grasps[k].angle<<"." << endl; 
  }

  out.clear();

  //cout<<msg2->bounding_boxes[0].Class<<endl;
  //cout<<objects[0].obj<<objects[0].center_x<<objects[0].center_y<<endl;

  cout<<"copy that"<<endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pclproc");

  ros::NodeHandle nh;

  message_filters::Subscriber<PointCloud2> pcl_sub(nh, "/camera/depth/points", 1);
  message_filters::Subscriber<BoundingBoxes> yolo_sub(nh, "/darknet_ros/bounding_boxes", 1);
  message_filters::Subscriber<ObjectCount> count_sub(nh, "/darknet_ros/found_object", 1);
  
  typedef sync_policies::ApproximateTime<PointCloud2,
                                         BoundingBoxes,ObjectCount>
      syncPolicy;
  Synchronizer<syncPolicy> sync(syncPolicy(10), pcl_sub, yolo_sub,count_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2,_3));

  ros::spin();

  return 0;
}