基于分割的目标姿态识别

基于分割的方法去识别目标物的姿态

/*
本段代码主要实现的功能：
1.去除平面
2.去除其他杂乱点云
3.对目标进行有向包围盒计算
4.计算目标重心点;计算旋转矩阵;计算欧拉角ZYX;即先绕Z轴旋转角度，再绕新的Y轴旋转角度，最后绕新的X轴旋转角度

*/

/*
本段代码主要实现的功能：
    1.去除平面
    2.去除其他杂乱点云
    3.对目标进行有向包围盒计算
    4.计算目标重心点;计算旋转矩阵;计算欧拉角ZYX;即先绕Z轴旋转角度，再绕新的Y轴旋转角度，最后绕新的X轴旋转角度

*/

#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/visualization/pcl_visualizer.h>
#include <iostream>

#include <string>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common.h>
#include <vtkAutoInit.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/features/moment_of_inertia_estimation.h>

int color_bar[][3]=
{
   
        {
    255,0,0},
        {
    0,255,0 },
        {
    0,0,255 },
        {
    0,255,255 },
        {
    255,255,0 },
        {
    255,255,255 },
        {
    255,0,255 }
};

//写出一个函数，将无用点云手动删除
pcl::PointCloud<pcl::PointXYZ>::Ptr shanchu(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//pcl::PointCloud<pcl::PointXYZ>::Ptr
{
   
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_chuli;
    pcl::PointCloud<pcl::PointXYZ>::iterator index;
    index=cloud->begin();

    for (index=cloud->begin();index!=cloud->end();index++)
    {
   

        //cout<<cloud->points[nIndex].x<<endl;
         if( (index->_PointXYZ::x>-0.3364)&&(index->_PointXYZ::x<0.4071)
                 &&(index->_PointXYZ::y>-0.191)&&(index->_PointXYZ::y<0.1826)
                 &&(index->_PointXYZ::z>0.68)&&(index->_PointXYZ::z<0.93) )
         {
   
             //std::cout<<"x "<<index->_PointXYZ::x<<"y "<<index->_PointXYZ::y<<"z "<<index->_PointXYZ::z<< std::endl;
             continue ;
         }
         else
         {
   
             //cloud->erase(index);
             index->_PointXYZ::x=0;
             index->_PointXYZ::y=0;
             index->_PointXYZ::z=0;
         }
    }
    cloud_chuli=cloud;
    /*
    //--show-----
    pcl::visualization::PCLVisualizer viewer("baolichudian");
    viewer.addPointCloud(cloud, "scene_cloud");//可视化场景点云
    while (!viewer.wasStopped())
    {
            viewer.spinOnce(100);
    }
    //------------
    */
  return cloud;
}
pcl::visualization::PCLVisualizer::Ptr viewer_left(new pcl::visualization::PCLVisualizer("segmention")); //left object
pcl::visualization::PCLVisualizer::Ptr viewer_right(new pcl::visualization::PCLVisualizer("segmention"));//right object
boost::shared_ptr<pcl::visualization::PCLVisualizer> obb(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//针对所有点云OBB有向包围盒  pcl::visualization::PCLVisualizer::Ptr
{