利用点云进行姿态检测估计

已于 2023-03-17 16:42:06 修改
阅读量1.1k 收藏 15
点赞数
文章标签： opencv 目标检测 计算机视觉
于 2020-07-11 10:16:58 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/yyd__/article/details/107280760
版权

直接保存一下code

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

*/
#include "ros/ros.h"
#include "std_msgs/String.h"

#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>

#include<geometry_msgs/Pose.h>
#include<std_msgs/Float32.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>//pcd io

geometry_msgs::Pose left_obj_position_orien;
geometry_msgs::Pose right_obj_position_orien;

Eigen::Vector3f mass_center_left_obj;               //左目标重位置
Eigen::Matrix3f rotational_matrix_OBB_left_obj;     //左目标坐标系相对相机坐标系的旋转矩阵
Eigen::Vector3f mass_center_right_obj;              //右目标重位置
Eigen::Matrix3f rotational_matrix_OBB_right_obj;    //右目标坐标系相对相机坐标系的旋转矩阵


ros::Publisher angle_pub_right;
ros::Publisher pose_pub_right;
ros::Publisher angle_pub_left;
ros::Publisher pose_pub_left;

geometry_msgs::Pose get_object_position(Eigen::Vector3f center, Eigen::Matrix3f rotational_matrix_OBB);//函数声明一下

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;
}
geometry_msgs::Pose viewer_left;    //left object
geometry_msgs::Pose viewer_right;   //right object

pcl::visualization::PCLVisualizer::Ptr viewer_right_obb(new pcl::visualization::PCLVisualizer("OBB_left"));
pcl::visualization::PCLVisualizer::Ptr viewer_left_obb(new pcl::visualization::PCLVisualizer("OBB_left"));

int left_id_i=0;    //左边序列号
int right_id_i=0;   //右边序列号
char str_left[25];  //左边序列号id
char str_right[25]; //右边序列号id


geometry_msgs::Pose obb_left(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//针对所有点云OBB有向包围盒  pcl::visualization::PCLVisualizer::Ptr
{
        pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
        feature_extractor.setInputCloud(cloud);
        feature_extractor.compute();

        std::vector <float> moment_of_inertia;
        std::vector <float> eccentricity;
        pcl::PointXYZ min_point_OBB;
        pcl::PointXYZ max_point_OBB;
        pcl::PointXYZ position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;  //xuanzhuangjuzhen    //I can understand typedef Matrix<float,3,3> Matrix3f ,rows() , cols() , size() 分别返回行数，列数和 元素的个数
        float major_value, middle_value, minor_value;
        Eigen::Vector3f major_vector, middle_vector, minor_vector;
        Eigen::Vector3f mass_center;            //zhongxin

        feature_extractor.getMomentOfInertia(moment_of_inertia);
        feature_extractor.getEccentricity(eccentricity);
        feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
        feature_extractor.getEigenValues(major_value, middle_value, minor_value);    //计算最大，次大，最小的三个特征值
        feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//计算最大，次大，最小的三个特征值对应的特征向量
        feature_extractor.getMassCenter(mass_center);                                //计算重心位置坐标

        //绘制OBB包围盒
        //pcl::visualization::PCLVisualizer::Ptr viewer_left_obb(new pcl::visualization::PCLVisualizer("OBB_left"));
        left_id_i++;
        sprintf(str_left,"%d",left_id_i);//将int型 转换成char [] 字符串类型


        viewer_left_obb->setBackgroundColor(0, 0, 0);
        viewer_left_obb->addCoordinateSystem(0.6,0,0,0,"OBB_left",0);

        pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> RandomColor(cloud);//设置随机颜色
        viewer_left_obb->addPointCloud<pcl::PointXYZ>(cloud, RandomColor, "OBB_left");
        viewer_left_obb->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "OBB_left");

        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Eigen::Quaternionf quat(rotational_matrix_OBB);
        viewer_left_obb->addCube(position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB_left");
        viewer_left_obb->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB_left");

        pcl::PointXYZ center(mass_center(0), mass_center(1), mass_center(2));
        pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
        pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
        pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

        std::string major_eigen_vector;  std::string middle_eigen_vector;  std::string minor_eigen_vector;

        major_eigen_vector= "major eigen vector" ;    major_eigen_vector +=str_left;
        middle_eigen_vector="middle eigen vector";    middle_eigen_vector+=str_left;
        minor_eigen_vector= "minor eigen vector" ;    minor_eigen_vector +=str_left;


        viewer_left_obb->addLine(center, x_axis, 1.0f, 0.0f, 0.0f, major_eigen_vector );        //主成分
        viewer_left_obb->addLine(center, y_axis, 0.0f, 1.0f, 0.0f, middle_eigen_vector);
        viewer_left_obb->addLine(center, z_axis, 0.0f, 0.0f, 1.0f, minor_eigen_vector );

        viewer_left_obb->spinOnce(10);
        viewer_left_obb->removePointCloud("OBB_left");
        //viewer->updatePointCloud(cloud,"OBB_left");
        viewer_left_obb->removeAllShapes();

        viewer_left_obb->updatePointCloud(cloud,str_left);


        cout <<"(目标物)中心点坐标: "<<endl;
        cout<< mass_center << endl;//中心点
        cout<<endl;
        cout <<"(相机坐标到目标物坐标)旋转矩阵: "<<endl;
        cout << rotational_matrix_OBB << endl;//矩阵

        //----旋转矩阵转化成欧拉角---
        double r11=rotational_matrix_OBB(0,0);double r12=rotational_matrix_OBB(0,1);double r13=rotational_matrix_OBB(0,2);
        double r21=rotational_matrix_OBB(1,0);double r22=rotational_matrix_OBB(1,1);double r23=rotational_matrix_OBB(1,2);
        double r31=rotational_matrix_OBB(2,0);double r32=rotational_matrix_OBB(2,1);double r33=rotational_matrix_OBB(2,2);
        //cout<<"rotational_matrix_OBB(0,0)"<<rotational_matrix_OBB(0,0)<<endl;// It's no matter! good.
        double sy=sqrt(r11*r11+r21*r21);

        bool singular =sy<1e-6;// if float sy=sqrt(r11*r11+r21*r21) is 0
        double x, y, z;
        if (!singular)
        {
                x = atan2(r32, r33);
                y = atan2(-r31, sy);
                z = atan2(r21, r11);
        }
        else
        {
                x = atan2(-r23, r22);
                y = atan2(-r31, sy);
                z = 0;
        }
        Eigen::Vector3d Z_Y_X;
        Z_Y_X(2)=(x * (180.0 / M_PI));
        Z_Y_X(1)=(y * (180.0 / M_PI));
        Z_Y_X(0)=(z * (180.0 / M_PI));
        cout<<"z_: "<< Z_Y_X(0)<<" y_: "<< Z_Y_X(1)<<" x_: "<< Z_Y_X(2)<<endl;

        /*
        while (!viewer->wasStopped())
        {
                viewer->spinOnce(100);
        }
        */
        Eigen::Vector3d camera_coordinate;//计算目标物三个坐标轴相对相机坐标系的夹角，方便后面机械臂抓取

        //Eigen::Vector3f mass_center,Eigen::Matrix3f rotational_matrix_OBB
        //geometry_msgs::Pose get_object_position(Eigen::Vector3f center, Eigen::Matrix3f rotational_matrix_OBB)
        geometry_msgs::Pose goal=get_object_position(mass_center,rotational_matrix_OBB);

        //viewer->spinOnce(10);//show obj
        //viewer->removePointCloud("OBB-OBJ");
        //viewer->close();

    return goal;//返回一个对象
}

geometry_msgs::Pose obb_right(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)//针对所有点云OBB有向包围盒  pcl::visualization::PCLVisualizer::Ptr
{
        pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
        feature_extractor.setInputCloud(cloud);
        feature_extractor.compute();

        std::vector <float> moment_of_inertia;
        std::vector <float> eccentricity;
        pcl::PointXYZ min_point_OBB;
        pcl::PointXYZ max_point_OBB;
        pcl::PointXYZ position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;  //xuanzhuangjuzhen    //I can understand typedef Matrix<float,3,3> Matrix3f ,rows() , cols() , size() 分别返回行数，列数和 元素的个数
        float major_value, middle_value, minor_value;
        Eigen::Vector3f major_vector, middle_vector, minor_vector;
        Eigen::Vector3f mass_center;            //zhongxin

        feature_extractor.getMomentOfInertia(moment_of_inertia);
        feature_extractor.getEccentricity(eccentricity);
        feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
        feature_extractor.getEigenValues(major_value, middle_value, minor_value);    //计算最大，次大，最小的三个特征值
        feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//计算最大，次大，最小的三个特征值对应的特征向量
        feature_extractor.getMassCenter(mass_center);                                //计算重心位置坐标

        //绘制OBB包围盒
        //pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("OBB_right"));
        right_id_i++;
        sprintf(str_right,"%d",right_id_i);//将int型 转换成char [] 字符串类型

        viewer_right_obb->setBackgroundColor(0, 0, 0);
        viewer_right_obb->addCoordinateSystem(0.6,0,0,0,str_right,0);

        pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> RandomColor(cloud);//设置随机颜色
        viewer_right_obb->addPointCloud<pcl::PointXYZ>(cloud, RandomColor, str_right);
        viewer_right_obb->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, str_right);

        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Eigen::Quaternionf quat(rotational_matrix_OBB);
        viewer_right_obb->addCube(position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, str_right);
        viewer_right_obb->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, str_right);

        pcl::PointXYZ center(mass_center(0), mass_center(1), mass_center(2));
        pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
        pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
        pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

        std::string major_eigen_vector;  std::string middle_eigen_vector;  std::string minor_eigen_vector;

        major_eigen_vector= "major eigen vector" ;    major_eigen_vector +=str_right;
        middle_eigen_vector="middle eigen vector";    middle_eigen_vector+=str_right;
        minor_eigen_vector= "minor eigen vector" ;    minor_eigen_vector +=str_right;

        viewer_right_obb->addLine(center, x_axis, 1.0f, 0.0f, 0.0f, major_eigen_vector );        //主成分
        viewer_right_obb->addLine(center, y_axis, 0.0f, 1.0f, 0.0f, middle_eigen_vector);
        viewer_right_obb->addLine(center, z_axis, 0.0f, 0.0f, 1.0f, minor_eigen_vector );

        viewer_right_obb->spinOnce(10);
        viewer_right_obb->removePointCloud(str_right); viewer_right_obb->removePointCloud(major_eigen_vector); viewer_right_obb->removePointCloud(minor_eigen_vector);
        //viewer_right_obb->removePointCloud("major eigen vector");viewer_right_obb->removePointCloud("minor eigen vector");

        viewer_right_obb->removeAllShapes();

        viewer_right_obb->updatePointCloud(cloud,str_right);


        cout <<"(目标物)中心点坐标: "<<endl;
        cout<< mass_center << endl;//中心点
        cout<<endl;
        cout <<"(相机坐标到目标物坐标)旋转矩阵: "<<endl;
        cout << rotational_matrix_OBB << endl;//矩阵

        //----旋转矩阵转化成欧拉角---
        double r11=rotational_matrix_OBB(0,0);double r12=rotational_matrix_OBB(0,1);double r13=rotational_matrix_OBB(0,2);
        double r21=rotational_matrix_OBB(1,0);double r22=rotational_matrix_OBB(1,1);double r23=rotational_matrix_OBB(1,2);
        double r31=rotational_matrix_OBB(2,0);double r32=rotational_matrix_OBB(2,1);double r33=rotational_matrix_OBB(2,2);
        //cout<<"rotational_matrix_OBB(0,0)"<<rotational_matrix_OBB(0,0)<<endl;// It's no matter! good.
        double sy=sqrt(r11*r11+r21*r21);

        bool singular =sy<1e-6;// if float sy=sqrt(r11*r11+r21*r21) is 0
        double x, y, z;
        if (!singular)
        {
                x = atan2(r32, r33);
                y = atan2(-r31, sy);
                z = atan2(r21, r11);
        }
        else
        {
                x = atan2(-r23, r22);
                y = atan2(-r31, sy);
                z = 0;
        }
        Eigen::Vector3d Z_Y_X;
        Z_Y_X(2)=(x * (180.0 / M_PI));
        Z_Y_X(1)=(y * (180.0 / M_PI));
        Z_Y_X(0)=(z * (180.0 / M_PI));
        cout<<"z_: "<< Z_Y_X(0)<<" y_: "<< Z_Y_X(1)<<" x_: "<< Z_Y_X(2)<<endl;
        /*
        while (!viewer->wasStopped())
        {
                viewer->spinOnce(100);
        }
        */
        Eigen::Vector3d camera_coordinate;//计算目标物三个坐标轴相对相机坐标系的夹角，方便后面机械臂抓取

        //Eigen::Vector3f mass_center,Eigen::Matrix3f rotational_matrix_OBB
        //geometry_msgs::Pose get_object_position(Eigen::Vector3f center, Eigen::Matrix3f rotational_matrix_OBB)
        geometry_msgs::Pose goal=get_object_position(mass_center,rotational_matrix_OBB);

        //viewer->spinOnce(10);//show obj
        //viewer->removePointCloud("OBB-OBJ");
        //viewer->close();

    return goal;//返回一个对象
}

pcl::visualization::PCLVisualizer viewer_left_kou("left_obj");
pcl::visualization::PCLVisualizer viewer_right_kou("right_obj");
void fenge(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_baoli)
{

    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_baoli (new pcl::PointCloud<pcl::PointXYZ>);//点云进行暴力除点行动
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);

    //------------------------------------------------
    // 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 (cloud_baoli);  //cloud chang to cloud_baoli
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter (*cloud_filtered);
    std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());//滤除平面
    pcl::PCDWriter writer;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (0.02);

    int i=0, nr_points = (int) cloud_filtered->points.size ();
    while (cloud_filtered->points.size () > 0.3 * nr_points)
    {
          // Segment the largest planar component from the remaining cloud
          seg.setInputCloud (cloud_filtered);     //滤波后的数据
          seg.segment (*inliers, *coefficients);
          if (inliers->indices.size () == 0)
          {
                std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
                break;
          }

          // Extract the planar inliers from the input cloud
          pcl::ExtractIndices<pcl::PointXYZ> extract;
          extract.setInputCloud (cloud_filtered);
          extract.setIndices (inliers);
          extract.setNegative (false);

          // Write the planar inliers to disk
          extract.filter (*cloud_plane);
          std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

          // Remove the planar inliers, extract the rest
          extract.setNegative (true);
          extract.filter (*cloud_f);
          cloud_filtered = cloud_f;
    }

    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_filtered);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (0.02); //设置近邻搜索的搜索半径为2cm
    ec.setMinClusterSize (100);    //设置一个聚类需要的最少点数目为100
    ec.setMaxClusterSize (25000);  //设置一个聚类需要的最大点数目为25000
    ec.setSearchMethod (tree);     //设置点云的搜索机制
    ec.setInputCloud (cloud_filtered); //设置原始点云
    ec.extract (cluster_indices);  //从点云中提取聚类

    // 我们将要使用的颜色
    float bckgr_gray_level = 0.0;  // 黑色
    float txt_gray_lvl = 1.0 - bckgr_gray_level;
    int num = cluster_indices.size();

    int j = 0;
    char str[25];                              //这一个的作用是为了下面给每条线段　１　个唯一的id符，不然的画只能画出一条线
    char jilu_name_pcd[25];                    //以每个聚类点云个数来标记.pcd文件名字

    pcl::PointCloud<pcl::PointXYZ> cloud_sum;  //add two cluste point.

    pcl::PointCloud<pcl::PointXYZ> cloud_cluster_left;
    pcl::PointCloud<pcl::PointXYZ> cloud_cluster_right;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster_left_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster_right_ptr (new pcl::PointCloud<pcl::PointXYZ>);

    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    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>);

          int bianhao_number=0;

          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;
          bianhao_number=cloud_cluster->points.size ();   //每个聚类点数大小
          sprintf(jilu_name_pcd, "%d", bianhao_number);   // 把整形bianhao_number变为文件名string类型

          cloud_sum += *cloud_cluster;                    //PointCloud::Ptr—>PointCloud 类型转换, 然后将多片点云加在一起

          //pcl::visualization::PCLVisualizer::Ptr viewer_coord;//pcl::visualization::PCLVisualizer viewer("segmention");   // 可视化部分
          //---计算每个聚类的中心点在左边还是右边---
          double point_value_x=0.0;
          double point_value_x_mean=0.0;
          for (int i=0;i<bianhao_number;i++)
          {
              point_value_x+=cloud_cluster->points[i].x;
          }
          point_value_x_mean=(1.0/bianhao_number)*point_value_x;

          cout<<"point_value_x_mean:"<<point_value_x_mean<<endl;


          //----------------------------------
          if(point_value_x<0.0)
          {
              viewer_left=obb_left(cloud_cluster);   //得到全局变量左目标重心坐标，目标坐标系相对相机旋转矩阵
              cloud_cluster_left+=*cloud_cluster;
          }
          else
          {
              viewer_right=obb_right(cloud_cluster);//得到全局变量右目标重心坐标，目标坐标系相对相机旋转矩阵
              cloud_cluster_right+=*cloud_cluster;
          }


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

          sprintf(str, "%d", j);// 把整形l

          //pcl::visualization::PCLVisualizer viewer("segmention");                           // 可视化部分
          pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
                 cloud_in_color_h(cloud, color_bar[j][0], color_bar[j][1], color_bar[j][2]);//赋予显示点云的颜色



          //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptrr(new pcl::PointCloud<pcl::PointXYZ>);
          //cloud_ptrr=cloud_sum.makeShared();
          //viewer.addPointCloud(cloud_ptrr, cloud_in_color_h, str);//cloud_cluster

          j++;
    }
    //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptrr(new pcl::PointCloud<pcl::PointXYZ>);
    //cloud_ptrr=cloud_sum.makeShared();
    cloud_cluster_left_ptr=cloud_cluster_left.makeShared();
    cloud_cluster_right_ptr=cloud_cluster_right.makeShared();

    viewer_left_kou.addPointCloud(cloud_cluster_left_ptr,std::to_string(1));//cloud_cluster
    viewer_left_kou.addCoordinateSystem(0.6,0,0,0,std::to_string(1),0);  //模型坐标轴控制,(-0.5,0,0)为上面平移向量
    viewer_left_kou.spinOnce(10);
    viewer_right_kou.addPointCloud(cloud_cluster_right_ptr,std::to_string(2));//cloud_cluster
    viewer_right_kou.addCoordinateSystem(0.6,0,0,0,std::to_string(2),0);  //模型坐标轴控制,(-0.5,0,0)为上面平移向量
    viewer_right_kou.spinOnce(10);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
           cloud_in_color_h_left_obj(cloud, color_bar[0][0], color_bar[0][1], color_bar[0][2]);//赋予显示点云的颜色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
           cloud_in_color_h_right_obj(cloud, color_bar[1][0], color_bar[1][1], color_bar[1][2]);//赋予显示点云的颜色

    viewer_left_kou.updatePointCloud(cloud_cluster_left_ptr, cloud_in_color_h_left_obj, "0");
    viewer_right_kou.updatePointCloud(cloud_cluster_right_ptr,cloud_in_color_h_right_obj,"1");

    viewer_left_kou.removePointCloud(std::to_string(1));//移除当前所有点云
    viewer_right_kou.removePointCloud(std::to_string(2));//移除当前所有点云
}
geometry_msgs::Pose get_object_position(Eigen::Vector3f center, Eigen::Matrix3f rotational_matrix_OBB)
{

  geometry_msgs::Pose object_position;

  object_position.position.x=center(0);
  object_position.position.y=center(1);
  object_position.position.z=center(2);
  //---将旋转矩阵转换成四元数---
  double r11=rotational_matrix_OBB(0,0);  double r12=rotational_matrix_OBB(0,1);  double r13=rotational_matrix_OBB(0,2);
  double r21=rotational_matrix_OBB(1,0);  double r22=rotational_matrix_OBB(1,1);  double r23=rotational_matrix_OBB(1,2);
  double r31=rotational_matrix_OBB(2,0);  double r32=rotational_matrix_OBB(2,1);  double r33=rotational_matrix_OBB(2,2);
  double ww=0.5*(sqrt(r11+r22+r33+1));
  object_position.orientation.x=(r32-r23)/(4*ww);
  object_position.orientation.y=(r13-r31)/(4*ww);
  object_position.orientation.z=(r21-r12)/(4*ww);
  object_position.orientation.w=ww;
  //-------------------------
  return object_position;
}
void fabu_callb()
{
    pose_pub_left.publish(viewer_left);
    pose_pub_right.publish(viewer_right);
}
void callb_point(const sensor_msgs::PointCloud2ConstPtr& input)
{
  std::cout<<"callb_point~~"<<std::endl;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);        //cloud is the output

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudd (new pcl::PointCloud<pcl::PointXYZ>);
  cloudd=cloud.makeShared();              //PointCloud—>PointCloud::Ptr

  std::cout << "PointCloud before filtering has: " << cloudd->points.size () << " data points." << std::endl;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_baoli=shanchu(cloudd);

  fenge(cloudd,cloud_baoli);
  fabu_callb();
}


int main (int argc, char** argv)
{

      ros::init(argc, argv, "get_obj_pose");
      ros::NodeHandle nd;

      ros::Rate loop_rate(10);
      cout<<"x "<<viewer_left.position.x<<"y "<<viewer_left.position.y<<
            "z "<<viewer_left.position.z<<endl;
      cout<<"w:"<<viewer_left.orientation.w<<"x: "<<viewer_left.orientation.x<<"y: "<<
            viewer_left.orientation.y<<"z: "<<viewer_left.orientation.z<<endl;

      pose_pub_left=nd.advertise<geometry_msgs::Pose>("/zr300_camera/goal_pose_left",100);      //左目标位姿话题
      pose_pub_right=nd.advertise<geometry_msgs::Pose>("/zr300_camera/goal_pose_right",100);  //右目标位姿话题

      pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>()); //定义模型点云类型
      ros::Subscriber pointPCD = nd.subscribe("/camera/points",1,callb_point);

      //ros::MultiThreadedSpinner spinner(4);

      while (ros::ok())
      {
          ros::spinOnce();
          //spinner.spin();
          loop_rate.sleep();
      }
   return (0);
}
yyd__
关注
  • 0
    点赞
  • 踩
  • 15
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
【C++PCL】点云处理稳健姿态估计配准
weixin_45196569的博客
06-23 165
点云加载: 从文件中加载源点云和目标点云，作为输入数据。下采样: 使用体素栅格滤波器对点云进行下采样，减少点的数量以提高计算效率。下采样通过将点云划分为固定大小的体素，然后用每个体素内的点的质心代替体素内所有点，从而减少点云的点数。
PCL计算点云位姿
最新发布
lddx_123456的博客
09-01 574
基于PCA计算点云位姿通常是指在三维空间中使用PCA（主成分分析）来估计点云数据的姿态或定位，即确定点云数据在三维空间中的位置（平移）和方向（旋转）。如下图所示通过PCA计算的平面点云位姿。
detectozord:利用PointNet + PCL从点云进行目标检测，分类和姿态估计
05-05
该存储库是用于在计算机视觉中使用pcl和tensorflow的沙箱（实验代码） “细分”是利用pcl从深度传感器中捕获3d点云，应用SAC细分以去除大型平面，通过pcl欧几里德聚类提取从场景中提取细分/数据/模型中预定义3D模型的候选实例，进行分类的管道他们使用点网将其放入预定义的对象集中，然后通过pcl SampleConsensusPrerejective估计其姿势 使用的软件包： python 3.5 x64 https://www.python.org/downloads/release/python-350/ pip install --upgrade https://storage.googleapis.com/tensorflow/windows/cpu/tensorflow-1.0.0-cp35-cp35m-win_amd64.whl pip install scipy
PCL点云姿态估计、刚性匹配SampleConsensusPrerejective
手口一斤
01-12 4326
目录 代码实现 结果分析和使用感受 点云刚性匹配，rigid objects匹配，假设模板是A点云，要在B点云中找到与A点云形状、大小都一致的点云集合； 代码实现 代码流程： 1、读入点云数据 2、对点云数据进行下采样 3、求点云数据的法向量 4、求点云数据的FPFH特征，其它特征也行 6、进行匹配 #include <Eigen/Core> #include <pcl/point_types.h> #include <pcl/point_cloud.h
姿态估计0-06：DenseFusion(6D姿态估计)-源码解析（2）-linemod数据集，预处理解读
热门推荐
江南才尽江南山，年少无知年少狂！
11-16 1万+
以下链接是个人关于DenseFusion(6D姿态估计) 所有见解，如有错误欢迎大家指出，我会第一时间纠正。有兴趣的朋友可以加微信：a944284742相互讨论技术。若是帮助到了你什么，一定要记得点赞！因为这是对我最大的鼓励。 姿态估计0-00：DenseFusion(6D姿态估计)-目录-史上最新无死角讲解https://blog.csdn.net/weixin_43013761/article......
点云物体及六自由度姿态估计笔记
zhiwen1225的博客
03-15 3828
一、研究背景 https://mp.weixin.qq.com/s/k9j-yZMDyJ3ZnBHBLI5dFQ 二、国内外研究现状 1.模板匹配 传统图片的模板到基于特征的模板： （1）三元组构成的模板： （2）快速方向距离倒角匹配 模板匹配的实时性受限于模板库物体种类和每类物体模板数量，再通过阈值判断。在堆叠杂乱场景鲁棒性不足。 2.形状匹配 3.点云配准：基于描述符的匹配 4.基于...
姿态估计算法汇总|基于RGB、RGB-D以及点云数据
小白学视觉
04-25 2448
点击上方“小白学视觉”，选择加"星标"或“置顶”重磅干货，第一时间送达本文转自计算机视觉life姿态估计算法汇总|基于RGB、RGB-D以及点云数据主要有整体方式、霍夫投票方式、Keypoint-based方式、Dense Correspondence方式等。实现方法：传统方法、深度学习方式。数据不同：RGB、RGB-D、点云数据等；标注工具LabelFusion：htt...
CVPR2020文章汇总 | 点云处理、三维重建、姿态估计、SLAM、3D数据集等(12篇)
3D视觉工坊
04-15 3805
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达1.PVN3D: A Deep Point-wise 3D Keypoints Voting Network for 6DoF Po...
论文解读 | 基于改进点对特征的点云6D姿态估计
Hinyeung2021的博客
05-31 665
对于体素单元中的每个点，比较其法线与20个代表性法线之间的角度，并考虑最小的一个作为它所属的角度单元(图4b)。针对Zhao等人[1]也提到的PCA结果方向模糊的问题，提出了一种基于奇偶规则的光线投射方法，用于检测单位法向量的端点是否位于封闭几何模型内。使用Kd-tree算法寻找每个模型点最近的场景点，并考虑与最近场景点的距离在阈值内的模型点作为拟合点。基于拟合点的验证步骤是改进的PPF方法中使用的一种方法，用于提高6D姿态估计的性能和效率。根据该方法的要求，一对中的两点不应该是交换的，即特征是不对称的。
涉及3D重建、SLAM、点云处理、3D检测与识别、深度估计、姿态估计等干货
XR风云
08-22 1219
1、3D重建 主要包括基于单目图像重建、结构光、双目重建、多目重建、多视图几何、深度相机、光场、SFM等内容~ 链接：Tom-Hardy-3D-Vision-Workshop/aweosme-3D-restruction 2、3D检测与识别 主要包括基于单目图像的3D检测、基于双目的3D检测、基于立体视觉的3D检测、基于融合数据的3D检测~ 链接：Tom-Hardy-3D-Vision-Workshop/awesome-3D-object-detection 3、点云处理 主要包括点云的滤..
使用VFH描述符进行聚类识别和6自由度姿态估计（大场景下点云数据的管道识别）
07-03
使用VFH描述符进行聚类识别和6自由度姿态估计（大场景下点云数据的管道识别）
点云 相机位姿_汉振分享 | 刚性物体6D位姿估计方法简述
weixin_28972031的博客
12-24 1689
引言在工业上的抓取作业中，多数情况下会有：待检测物体类型单一、物体模型表面纹理较少、工件散乱且密集、金属表面(油污、反光)影响成像效果等作业环境上的特殊性条件存在。针对不同的应用场景及作业需求，需要有的放矢地选择一种合适的位姿估计方案，以提高后续机器人的抓取成功率及作业的整体效率。现阶段，针对刚性物体的位姿估计方法，大致可以归为以下三类：1 基于模板匹配2 基于3D描述符(描述子)3 基于学习(神...
【2019ICRA】PointNetGPD: 基于PointNet直接从点云中估计抓取位姿
dsoftware的博客
07-24 9066
摘要 本文发表于2019年ICRA，作者提出了一种端到端的抓取位置预测模型，能够从点云中估计出机器人的抓取位姿。和当前算法相比，作者提出的PointNetGPD是一种轻量级的网络模型，能够处理抓取器闭合区域的点云，进行抓取质量估计。网络以原始点云作为输入，能够捕捉到抓取器闭合区域点云的复杂几何结构，即使这些点云很稀疏。此外，作者基于YCB模型库，生成了350K包含真实点云和抓取的数据集用于训练。提...
Open3D 点云配准：姿态图（阅读经典）
dayuhaitang1的博客
03-11 185
Open3D 点云配准：姿态图（阅读经典）
PCL点云：点云显示&观察视角姿态
weixin_42585456的博客
11-06 7355
导入头文件 #include <iostream> #include <pcl/point_cloud.h> #include <pcl/visualization/pcl_visualizer.h> 该段代码用来测试角度 红色是X轴，绿色是Y轴，蓝色是Z，也就是说PCL点云库中使用的是右手三维坐标系 //创建点云对象指针和引用 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::Poi.
5篇CVPR 各路大佬显身手 点云分割、姿态估计、物体检测、生成重建
3D视觉工坊
04-07 3229
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达3D方向：港中文针对分割问题提出双重设置点分组模型；清华提出无需 PoseNet 的联合深度姿势学习；北理工和百度提出基于LiDA...
VoxelNet：基于点云的端到端 3D 物体检测网络
xiaohu的博客
03-31 530
文章目录网络结构特征学习网络Voxel PartitionGroupingRandom SampllingVoxel Feature Encoding，VFESparse Tensor Representation卷积中间层区域提出网络（RPN）损失函数 网络结构 VoxelNet 将点云划分为等间隔的3D体素（Voxel），并通过新引入的体素特征编码（VFE）层将每个体素内的一组点转换为单一特征表示。通过这种方式，点云被编码为具有描述性的体积表示，然后将其连接到 RPN 以生成检测结果。 VoxelNet
基于三维点云的汽车检测技术的研究现状
04-27
目前，基于三维点云的汽车检测技术已经成为了自动驾驶和智能交通领域的研究热点。该技术主要是通过激光雷达或者摄像头等传感器获取车辆周围的三维点云数据，然后利用计算机视觉和深度学习等技术对点云数据进行处理和分析，从而实现对车辆的检测和识别。 目前，基于三维点云的汽车检测技术主要包括两种方法：基于传统计算机视觉算法的方法和基于深度学习的方法。其中，基于传统计算机视觉算法的方法主要包括点云分割、聚类和拟合等技术，可以实现对车辆的定位和姿态估计等功能。而基于深度学习的方法，则可以利用神经网络对点云数据进行端到端的处理和分析，从而实现对车辆的快速和准确的检测和识别。 目前，基于三维点云的汽车检测技术已经在自动驾驶、智能交通和智能交通安全等领域得到了广泛应用，未来还将继续发展和完善，为智能交通系统的实现和智能交通安全的保障提供更加强大的技术支持。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

yyd__

你的打赏和鼓励我会珍惜！

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值