求解平面上物体的有向3d包围盒

算法流程:
(1)点云下采样(体素滤波);
(2)ransac算法分割拟合地面平面;
(3)裁剪工作区域(指定空间中四个点,裁剪点云只保留在(2)中平面上的投影在四边形内部的点);
(4)再用ransac算法去除多余平面;
(5)Euclidean聚类算法分割出目标物体的点云簇;
(6)通过包围盒算法计算包围盒。

改进OBB包围盒

由于物体是放在地面上,因此可以利用地面平面的法向量约束物体包围盒的朝向。具体做法如下:

  1. 把物体点云投影到桌面平面上得到底面投影点云;
  2. 计算底面的obb外接矩形,得到物体包围盒的朝向从而建立局部坐标系;
  3. 将物体点云投影到局部坐标系z轴上得到投影轴线的点云;
  4. 投影得到点云的中心作为物体包围盒的中心;
  5. 通过物体包围盒的朝向和中心确定其包围盒。
#include 
#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include 
#include  
#include  
#include  
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define DEBUG
#define VIEWER


//包围盒结构体
struct BoundingBox
{
	Eigen::Vector3f center;
	Eigen::Quaternionf quat;
	float width;
	float height;
	float depth;
};


//判断点是否在四边形工作区域内
bool point_in_quadrangle(pcl::PointXYZ p, pcl::PointCloud<pcl::PointXYZ>::Ptr pts)
{
	float a = (pts->points[1].x - pts->points[0].x) * (p.y - pts->points[0].y) - (pts->points[1].y - pts->points[0].y) * (p.x - pts->points[0].x);
	float b = (pts->points[2].x - pts->points[1].x) * (p.y - pts->points[1].y) - (pts->points[2].y - pts->points[1].y) * (p.x - pts->points[1].x);
	float c = (pts->points[3].x - pts->points[2].x) * (p.y - pts->points[2].y) - (pts->points[3].y - pts->points[2].y) * (p.x - pts->points[2].x);
	float d = (pts->points[0].x - pts->points[3].x) * (p.y - pts->points[3].y) - (pts->points[0].y - pts->points[3].y) * (p.x - pts->points[3].x);

	return (a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0);
}


int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::io::loadPCDFile("cloud.pcd", *cloud);	

	if (!cloud->size())
	{
		std::cout << "point cloud is empty!" << std::endl;
		return -1;
	}
	std::cout << "points nums:" << cloud->size() << std::endl;


	/*体素滤波*/
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::VoxelGrid<pcl::PointXYZRGB> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(10, 10, 10);
	vg.filter(*cloud_filtered);
	std::cout << "points nums after filter:" << cloud_filtered->size() << std::endl;

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_filtered.pcd", *cloud_filtered);
#endif

	/*ransac分割地面*/
	pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>(cloud_filtered));
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
	std::vector<int> inliers_indices;
	Eigen::VectorXf coefficients;

	pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_plane);
	ransac.setDistanceThreshold(10);	
	ransac.computeModel();						
	ransac.getInliers(inliers_indices);		
	ransac.getModelCoefficients(coefficients);

	pcl::copyPointCloud(*cloud_filtered, inliers_indices, *cloud_plane);

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_plane.pcd", *cloud_plane);
#endif


	/*裁剪工作区域*/
	Eigen::Matrix3f rot = Eigen::Quaternionf::FromTwoVectors( \
		Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]), \
		Eigen::Vector3f(0, 0, 1)).toRotationMatrix();

	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
	transformation_matrix.topLeftCorner(3, 3) = rot;

	Eigen::MatrixXf plane_origin(4, 1), plane_transformed(4, 1);
	plane_origin << coefficients[0], coefficients[1], coefficients[2], coefficients[3];
	plane_transformed = transformation_matrix * plane_origin;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::transformPointCloud(*cloud_filtered, *cloud_transformed, transformation_matrix);

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_transformed.pcd", *cloud_transformed);
#endif

	pcl::PointCloud<pcl::PointXYZ>::Ptr workspace_points3d(new pcl::PointCloud<pcl::PointXYZ>);
	workspace_points3d->push_back(pcl::PointXYZ(-720, 486, 1564));
	workspace_points3d->push_back(pcl::PointXYZ(463, 492, 1543));
	workspace_points3d->push_back(pcl::PointXYZ(427, 118, 2714));
	workspace_points3d->push_back(pcl::PointXYZ(-750, 76, 2825));
	pcl::transformPointCloud(*workspace_points3d, *workspace_points3d, transformation_matrix);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_workspace(new pcl::PointCloud<pcl::PointXYZ>);
	for (size_t i = 0; i < cloud_transformed->size(); i++)
	{
		pcl::PointXYZ p(cloud_transformed->points[i].x, cloud_transformed->points[i].y, cloud_transformed->points[i].z);
		if (point_in_quadrangle(p, workspace_points3d))
		{
			cloud_workspace->push_back(p);
		}
	}

	if (!cloud_workspace->size())
	{
		std::cout << "get workspace failed!" << std::endl;
		return -1;
	}

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_workspace.pcd", *cloud_workspace);
#endif


	/*目标物体分割*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segmented(new pcl::PointCloud<pcl::PointXYZ>);

	//for (size_t i = 0; i < cloud_workspace->size(); i++)
	//{
	//	Eigen::Vector4f vec(plane_transformed(0, 0), plane_transformed(1, 0), plane_transformed(2, 0), plane_transformed(3, 0));
	//	if (pcl::pointToPlaneDistance(cloud_workspace->points[i], vec) > 20)
	//	{
	//		cloud_segmented->push_back(cloud_workspace->points[i]);
	//	}
	//}

	pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(20);
	seg.setInputCloud(cloud_workspace);
	seg.segment(*inliers, *plane_coefficients);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud_workspace);
	extract.setIndices(inliers);
	extract.setNegative(true);
	extract.filter(*cloud_segmented);

	std::cout << "points nums after segment:" << cloud_segmented->size() << std::endl;
	if (!cloud_segmented->size())
	{
		std::cout << "segment failed!" << std::endl;
		return -1;
	}

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_segmented.pcd", *cloud_segmented);
#endif


	/*Euclidean聚类*/
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud_segmented);

	pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
	std::vector<pcl::PointIndices> clusters;
	clustering.setClusterTolerance(20);
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(10000);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(cloud_segmented);
	clustering.extract(clusters);

	int cloud_num = 0;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_cluster;
	std::vector<float> cloud_distance;
	for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); it++)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator i = it->indices.begin(); i != it->indices.end(); i++)
		{
			cluster->points.push_back(cloud_segmented->points[*i]);
		}
		cluster->height = 1;
		cluster->width = cluster->points.size();
		std::cout << "points nums of cluster" << std::to_string(cloud_num) + ": " << cluster->points.size() << std::endl;

#ifdef DEBUG
		pcl::io::savePCDFile("cluster" + std::to_string(cloud_num) + ".pcd", *cluster);
#endif
		cloud_num++;
		cloud_cluster.push_back(cluster);
	}


//	/*统计学滤波*/
//	pcl::PointCloud::Ptr cloud_object_filter(new pcl::PointCloud);
//	pcl::StatisticalOutlierRemoval sor;
//	sor.setInputCloud(cloud_object);
//	sor.setMeanK(50); //K近邻搜索点个数
//	sor.setStddevMulThresh(0.5); //标准差倍数
//	sor.setNegative(false); //保留未滤波点(内点)
//	sor.filter(*cloud_object_filter);  //保存滤波结果到cloud_filter
//
//#ifdef SAVE_CLOUD
//	pcl::io::savePCDFile("cloud_object_filter.pcd", *cloud_object_filter);
//#endif
//

	/*计算包围盒*/
	std::vector<BoundingBox> boxes;
	for (size_t i = 0; i < cloud_cluster.size(); i++)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_object(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud_cluster[i], *cloud_object);

		//把物体投影到桌面得到底面
		pcl::ProjectInliers<pcl::PointXYZ> pro;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

		pro.setModelType(pcl::SACMODEL_PLANE);
		pro.setInputCloud(cloud_object);
		pro.setModelCoefficients(plane_coefficients);
		pro.filter(*cloud_projected);

#ifdef  DEBUG
		pcl::io::savePCDFile("cloud_projected.pcd", *cloud_projected);
#endif 

		pcl::transformPointCloud(*cloud_object, *cloud_object, transformation_matrix.inverse());
		pcl::transformPointCloud(*cloud_projected, *cloud_projected, transformation_matrix.inverse());


		//计算底面的外接矩形
		pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extactor;
		feature_extactor.setInputCloud(cloud_projected);
		feature_extactor.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 roatation_maxtrix_obb;

		feature_extactor.getMomentOfInertia(moment_of_inertia);
		feature_extactor.getEccentricity(eccentricity);
		feature_extactor.getOBB(min_point_obb, max_point_obb, position_obb, roatation_maxtrix_obb);

		Eigen::Quaternionf quat(roatation_maxtrix_obb);
		Eigen::Vector3f x_axis = quat.toRotationMatrix() * Eigen::Vector3f(1, 0, 0);
		Eigen::Vector3f y_axis = quat.toRotationMatrix() * Eigen::Vector3f(0, 1, 0);
		Eigen::Vector3f z_axis = quat.toRotationMatrix() * Eigen::Vector3f(0, 0, 1);

		//将物体点云投影到z轴
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_axis(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::ModelCoefficients::Ptr coefficients_line(new pcl::ModelCoefficients);
		coefficients_line->values.resize(6);
		coefficients_line->values[0] = position_obb.x;
		coefficients_line->values[1] = position_obb.y;
		coefficients_line->values[2] = position_obb.z;
		coefficients_line->values[3] = z_axis[0];
		coefficients_line->values[4] = z_axis[1];
		coefficients_line->values[5] = z_axis[2];

		pro.setModelType(pcl::SACMODEL_LINE);
		pro.setInputCloud(cloud_object);
		pro.setModelCoefficients(coefficients_line);
		pro.filter(*cloud_axis);

#ifdef  DEBUG
		pcl::io::savePCDFile("cloud_axis.pcd", *cloud_axis);
#endif  

		int top_index = -1, down_index = -1;
		float top_z = std::numeric_limits <float>::min(), down_z = std::numeric_limits <float>::max();
		for (size_t i = 0; i < cloud_axis->size(); i++)
		{
			if (cloud_axis->points[i].z > top_z)
			{
				top_z = cloud_axis->points[i].z;
				top_index = i;
			}
			if (cloud_axis->points[i].z < down_z)
			{
				down_z = cloud_axis->points[i].z;
				down_index = i;
			}
		}

		//投影得到点云的中心作为包围盒中心
		Eigen::Vector3f center;
		center.x() = (cloud_axis->points[top_index].x + cloud_axis->points[down_index].x) / 2;
		center.y() = (cloud_axis->points[top_index].y + cloud_axis->points[down_index].y) / 2;
		center.z() = (cloud_axis->points[top_index].z + cloud_axis->points[down_index].z) / 2;

#ifdef DEBUG
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center(new pcl::PointCloud<pcl::PointXYZ>);
		cloud_center->push_back(pcl::PointXYZ(center.x(), center.y(), center.z()));
		pcl::io::savePCDFile("cloud_center.pcd", *cloud_center);
#endif 

		BoundingBox box;
		box.center = center;
		box.quat = quat;
		box.width = max_point_obb.x - min_point_obb.x;
		box.height = max_point_obb.y - min_point_obb.y;
		box.depth = pcl::euclideanDistance(cloud_axis->points[top_index], cloud_axis->points[down_index]);
		std::cout << "box.width:" << box.width << " box.height:" << box.height << " box.depth:" << box.depth << std::endl;

		boxes.push_back(box);
	}
	
	/*结果可视化*/
#ifdef VIEWER
	pcl::visualization::PCLVisualizer viewer("viewer");
	viewer.addPointCloud(cloud, "cloud");
	for (size_t i = 0; i < boxes.size(); i++)
	{
		viewer.addCube(boxes[i].center, boxes[i].quat, boxes[i].width, boxes[i].height, boxes[i].depth, std::to_string(i));
	}
	viewer.setRepresentationToWireframeForAllActors();

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
	}
#endif 

	return 0;
}

可视化结果:
求解平面上物体的有向3d包围盒_第1张图片

最小投影面积包围盒

具体做法如下:

  1. 把物体点云投影到桌面平面上得到底面投影点云;
  2. 计算底面投影点云点集的最小外包旋转矩形,得到物体包围盒的朝向从而建立局部坐标系;
  3. 将物体点云投影到局部坐标系z轴上得到投影轴线的点云;
  4. 投影得到点云的中心作为物体包围盒的中心;
  5. 通过物体包围盒的朝向和中心确定其包围盒。
#include 
#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include 
#include  
#include  
#include  
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define DEBUG
#define VIEWER


//包围盒结构体
struct BoundingBox
{
	Eigen::Vector3f center;
	Eigen::Quaternionf quat;
	float width;
	float height;
	float depth;
};


//判断点是否在四边形工作区域内
bool point_in_quadrangle(pcl::PointXYZ p, pcl::PointCloud<pcl::PointXYZ>::Ptr pts)
{
	float a = (pts->points[1].x - pts->points[0].x) * (p.y - pts->points[0].y) - (pts->points[1].y - pts->points[0].y) * (p.x - pts->points[0].x);
	float b = (pts->points[2].x - pts->points[1].x) * (p.y - pts->points[1].y) - (pts->points[2].y - pts->points[1].y) * (p.x - pts->points[1].x);
	float c = (pts->points[3].x - pts->points[2].x) * (p.y - pts->points[2].y) - (pts->points[3].y - pts->points[2].y) * (p.x - pts->points[2].x);
	float d = (pts->points[0].x - pts->points[3].x) * (p.y - pts->points[3].y) - (pts->points[0].y - pts->points[3].y) * (p.x - pts->points[3].x);

	return (a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0);
}


int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::io::loadPCDFile("cloud.pcd", *cloud);

	if (!cloud->size())
	{
		std::cout << "point cloud is empty!" << std::endl;
		return -1;
	}
	std::cout << "points nums:" << cloud->size() << std::endl;


	/*体素滤波*/
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::VoxelGrid<pcl::PointXYZRGB> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(10, 10, 10);
	vg.filter(*cloud_filtered);
	std::cout << "points nums after filter:" << cloud_filtered->size() << std::endl;

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_filtered.pcd", *cloud_filtered);
#endif

	/*ransac分割地面*/
	pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>(cloud_filtered));
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
	std::vector<int> inliers_indices;
	Eigen::VectorXf coefficients;

	pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_plane);
	ransac.setDistanceThreshold(10);
	ransac.computeModel();
	ransac.getInliers(inliers_indices);
	ransac.getModelCoefficients(coefficients);

	pcl::copyPointCloud(*cloud_filtered, inliers_indices, *cloud_plane);

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_plane.pcd", *cloud_plane);
#endif


	/*裁剪工作区域*/
	Eigen::Matrix3f rot = Eigen::Quaternionf::FromTwoVectors(\
		Eigen::Vector3f(coefficients[0], coefficients[1], coefficients[2]), \
		Eigen::Vector3f(0, 0, 1)).toRotationMatrix();

	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();
	transformation_matrix.topLeftCorner(3, 3) = rot;

	Eigen::MatrixXf plane_origin(4, 1), plane_transformed(4, 1);
	plane_origin << coefficients[0], coefficients[1], coefficients[2], coefficients[3];
	plane_transformed = transformation_matrix * plane_origin;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::transformPointCloud(*cloud_filtered, *cloud_transformed, transformation_matrix);

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_transformed.pcd", *cloud_transformed);
#endif

	pcl::PointCloud<pcl::PointXYZ>::Ptr workspace_points3d(new pcl::PointCloud<pcl::PointXYZ>);
	workspace_points3d->push_back(pcl::PointXYZ(-720, 486, 1564));
	workspace_points3d->push_back(pcl::PointXYZ(463, 492, 1543));
	workspace_points3d->push_back(pcl::PointXYZ(427, 118, 2714));
	workspace_points3d->push_back(pcl::PointXYZ(-750, 76, 2825));
	pcl::transformPointCloud(*workspace_points3d, *workspace_points3d, transformation_matrix);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_workspace(new pcl::PointCloud<pcl::PointXYZ>);
	for (size_t i = 0; i < cloud_transformed->size(); i++)
	{
		pcl::PointXYZ p(cloud_transformed->points[i].x, cloud_transformed->points[i].y, cloud_transformed->points[i].z);
		if (point_in_quadrangle(p, workspace_points3d))
		{
			cloud_workspace->push_back(p);
		}
	}

	if (!cloud_workspace->size())
	{
		std::cout << "get workspace failed!" << std::endl;
		return -1;
	}

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_workspace.pcd", *cloud_workspace);
#endif


	/*目标物体分割*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segmented(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(20);
	seg.setInputCloud(cloud_workspace);
	seg.segment(*inliers, *plane_coefficients);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud_workspace);
	extract.setIndices(inliers);
	extract.setNegative(true);
	extract.filter(*cloud_segmented);

	std::cout << "points nums after segment:" << cloud_segmented->size() << std::endl;
	if (!cloud_segmented->size())
	{
		std::cout << "segment failed!" << std::endl;
		return -1;
	}

#ifdef DEBUG
	pcl::io::savePCDFile("cloud_segmented.pcd", *cloud_segmented);
#endif


	/*Euclidean聚类*/
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud_segmented);

	pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
	std::vector<pcl::PointIndices> clusters;
	clustering.setClusterTolerance(20);
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(10000);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(cloud_segmented);
	clustering.extract(clusters);

	int cloud_num = 0;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_cluster;
	std::vector<float> cloud_distance;
	for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); it++)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator i = it->indices.begin(); i != it->indices.end(); i++)
		{
			cluster->points.push_back(cloud_segmented->points[*i]);
		}
		cluster->height = 1;
		cluster->width = cluster->points.size();
		std::cout << "points nums of cluster" << std::to_string(cloud_num) + ": " << cluster->points.size() << std::endl;

#ifdef DEBUG
		pcl::io::savePCDFile("cluster" + std::to_string(cloud_num) + ".pcd", *cluster);
#endif
		cloud_num++;
		cloud_cluster.push_back(cluster);
	}

	/*计算包围盒*/
	std::vector<BoundingBox> boxes;
	for (size_t i = 0; i < cloud_cluster.size(); i++)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_object(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud_cluster[i], *cloud_object);

		//把物体投影到桌面得到底面
		pcl::ProjectInliers<pcl::PointXYZ> pro;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

		pro.setModelType(pcl::SACMODEL_PLANE);
		pro.setInputCloud(cloud_object);
		pro.setModelCoefficients(plane_coefficients);
		pro.filter(*cloud_projected);

#ifdef  DEBUG
		pcl::io::savePCDFile("cloud_projected.pcd", *cloud_projected);
#endif 

		//计算点集的最小外包旋转矩形
		std::vector<cv::Point2f> points;
		for (size_t i = 0; i < cloud_projected->size(); i++)
		{
			points.push_back(cv::Point2f(cloud_projected->points[i].x, cloud_projected->points[i].y));
		}
		cv::RotatedRect rect = cv::minAreaRect(points);
		cv::Mat vertices;
		cv::boxPoints(rect, vertices);

#ifdef DEBUG
		cv::Mat img(1000, 1000, CV_8UC3, cv::Scalar(255, 255, 255));
		for (int i = 0; i < points.size(); ++i)
		{
			cv::circle(img, cv::Point(points[i].x + 500, points[i].y + 2500), 1, cv::Scalar(0, 0, 255), -1);
		}

		for (int i = 0; i < vertices.rows; ++i)
		{
			cv::Point p1 = cv::Point(vertices.at<float>(i, 0) + 500, vertices.at<float>(i, 1) + 2500);
			cv::Point p2 = cv::Point(vertices.at<float>((i + 1) % 4, 0) + 500, vertices.at<float>((i + 1) % 4, 1) + 2500);
			cv::line(img, p1, p2, cv::Scalar(255, 0, 0), 1);
		}
		cv::imwrite("RotatedRect.bmp", img);
#endif 

		cv::Point p0 = cv::Point(vertices.at<float>(0, 0), vertices.at<float>(0, 1));
		cv::Point p1 = cv::Point(vertices.at<float>(1, 0), vertices.at<float>(1, 1));
		cv::Point p2 = cv::Point(vertices.at<float>(2, 0), vertices.at<float>(2, 1));
		Eigen::Vector3f rect_axis(p0.x - p1.x, p0.y - p1.y, 0);

		Eigen::Matrix3f box_rot = Eigen::Quaternionf::FromTwoVectors(rect_axis, Eigen::Vector3f(1, 0, 0)).toRotationMatrix() * rot;
		Eigen::Quaternionf quat(box_rot.inverse());

		//将物体点云投影到z轴
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_axis(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::ModelCoefficients::Ptr coefficients_line(new pcl::ModelCoefficients);
		coefficients_line->values.resize(6);
		coefficients_line->values[0] = (p0.x + p2.x) / 2;
		coefficients_line->values[1] = (p0.y + p2.y) / 2;
		coefficients_line->values[2] = 0;
		coefficients_line->values[3] = 0;
		coefficients_line->values[4] = 0;
		coefficients_line->values[5] = 1;

		pro.setModelType(pcl::SACMODEL_LINE);
		pro.setInputCloud(cloud_object);
		pro.setModelCoefficients(coefficients_line);
		pro.filter(*cloud_axis);

#ifdef  DEBUG
		pcl::io::savePCDFile("cloud_axis.pcd", *cloud_axis);
#endif  

		int top_index = -1, down_index = -1;
		float top_z = std::numeric_limits <float>::min(), down_z = std::numeric_limits <float>::max();
		for (size_t i = 0; i < cloud_axis->size(); i++)
		{
			if (cloud_axis->points[i].z > top_z)
			{
				top_z = cloud_axis->points[i].z;
				top_index = i;
			}
			if (cloud_axis->points[i].z < down_z)
			{
				down_z = cloud_axis->points[i].z;
				down_index = i;
			}
		}

		//投影得到点云的中心作为包围盒中心
		pcl::PointXYZ center;
		center.x = (cloud_axis->points[top_index].x + cloud_axis->points[down_index].x) / 2;
		center.y = (cloud_axis->points[top_index].y + cloud_axis->points[down_index].y) / 2;
		center.z = (cloud_axis->points[top_index].z + cloud_axis->points[down_index].z) / 2;

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center(new pcl::PointCloud<pcl::PointXYZ>);
		cloud_center->push_back(center);

#ifdef DEBUG
		pcl::io::savePCDFile("cloud_center.pcd", *cloud_center);
#endif 

		pcl::transformPointCloud(*cloud_object, *cloud_object, transformation_matrix.inverse());
		pcl::transformPointCloud(*cloud_projected, *cloud_projected, transformation_matrix.inverse());
		pcl::transformPointCloud(*cloud_center, *cloud_center, transformation_matrix.inverse());

		BoundingBox box;
		box.center = cloud_center->points[0].getVector3fMap();
		box.quat = quat;
		box.width = cv::norm(p0 - p1);
		box.height = cv::norm(p1 - p2);
		box.depth = pcl::euclideanDistance(cloud_axis->points[top_index], cloud_axis->points[down_index]);
		std::cout << "box.width:" << box.width << " box.height:" << box.height << " box.depth:" << box.depth << std::endl;

		boxes.push_back(box);
	}
	

	/*结果可视化*/
#ifdef VIEWER
	pcl::visualization::PCLVisualizer viewer("viewer");
	viewer.addPointCloud(cloud, "cloud");
	for (size_t i = 0; i < boxes.size(); i++)
	{
		viewer.addCube(boxes[i].center, boxes[i].quat, boxes[i].width, boxes[i].height, boxes[i].depth, std::to_string(i));
	}
	viewer.setRepresentationToWireframeForAllActors();

	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
	}
#endif 

	return 0;
}

可视化结果:
求解平面上物体的有向3d包围盒_第2张图片

你可能感兴趣的:(6D,pose,estimation,平面,3d,点云,包围盒,位姿估计)