直接保存一下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]; //这一个的作用是为了下面给每条线段 1 个唯一的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);
}