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

最新推荐文章于 2023-11-26 20:56:49 发布
最新推荐文章于 2023-11-26 20:56:49 发布
阅读量297 收藏 4
点赞数 1
分类专栏： 6D pose estimation 文章标签： 平面 3d 点云 包围盒 位姿估计
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/taifyang/article/details/131144254
版权

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

改进OBB包围盒

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

  1. 把物体点云投影到桌面平面上得到底面投影点云；
  2. 计算底面的obb外接矩形，得到物体包围盒的朝向从而建立局部坐标系；
  3. 将物体点云投影到局部坐标系z轴上得到投影轴线的点云；
  4. 投影得到点云的中心作为物体包围盒的中心；
  5. 通过物体包围盒的朝向和中心确定其包围盒。
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigenvalues>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h> 
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/statistical_outlier_removal.h> 
#include <pcl/sample_consensus/model_types.h> 
#include <pcl/sample_consensus/method_types.h> 
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/common/distances.h>
#include <pcl/visualization/pcl_visualizer.h>

#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<pcl::PointXYZRGB>::Ptr cloud_object_filter(new pcl::PointCloud<pcl::PointXYZRGB>);
//	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> 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;
}

可视化结果：
在这里插入图片描述

最小投影面积包围盒

具体做法如下：

  1. 把物体点云投影到桌面平面上得到底面投影点云；
  2. 计算底面投影点云点集的最小外包旋转矩形，得到物体包围盒的朝向从而建立局部坐标系；
  3. 将物体点云投影到局部坐标系z轴上得到投影轴线的点云；
  4. 投影得到点云的中心作为物体包围盒的中心；
  5. 通过物体包围盒的朝向和中心确定其包围盒。
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigenvalues>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h> 
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/statistical_outlier_removal.h> 
#include <pcl/sample_consensus/model_types.h> 
#include <pcl/sample_consensus/method_types.h> 
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/common/distances.h>
#include <pcl/visualization/pcl_visualizer.h>

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

可视化结果：
在这里插入图片描述

给算法爸爸上香
关注
  • 1
    点赞
  • 踩
  • 4
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
求包围盒体积_最小有向包围盒
weixin_33078391的博客
01-15 1129
chopper：目录​zhuanlan.zhihu.com本篇文章介绍一种线性复杂度的最小有向包围盒的估计算法，并在文章最后给出C++源码实现的关键代码。给定一个三维凸包，设有个三角形，记为，其中、、分别是三角形的3个顶点，求该凸包的最小有向包围盒。关于最小有向包围盒（OBB, Oriented Bounding Box）的创建方法有很多，O'Rourke[1]在1985年提出了一种复杂度是的精确...
VTK修炼之道46：图形基本操作进阶_三角网格体积、表面积、测地距离、包围盒
沈春旭的博客
02-02 9275
1.基本图形操作意义 图形处理，比如图形平滑、多分辨率分析、特征提取等都离不开一些基本的图形操作。掌握这些基本的图形操作有助于理解和深入学习图形处理和分析方法。 VTK中提供了多种图形的基本操作，其中最简单的是点的欧氏距离计算，可以使用vtkMath进行计算，也可以直接计算向量的模。一些图元类提供了许多可以方便使用的静态函数，如 vtkLine提供了点与线间的距离计算； vtkTria
OBB的计算python实现
juluwangriyue的博客
01-31 2254
OBB的经典生成算法：使用PCA（主成分分析）。主成分分析有一个关键的线性代数计算步骤，即求解协方差矩阵的特征值和特征向量，这一点必须使用数值分析算法而不能用解题用的基本行变换手段，因为现代程序最大的特点就是干一些枯燥重复的事情——迭代.在这里主要介绍三维的思路，黑盒模型：obb的参数（中心点、三轴向量、三轴半长，以确定一个空间中的矩形）= f（点集）
C++计算三维空间点的obb包围框，包含osg调用
老张的博客
10-27 498
C++计算obb包围盒的完整代码，渲染使用的是osg。
pythonocc进阶学习：包围盒boundingbox
weixin_42755384的博客
08-02 5606
一个物体是有自身的包围盒的，对于2维的线（edge）或者环（wire）来说，对于三维实体来说，其自身的包围盒也很重要，那么包围盒究竟有什么作用呢？对于屏幕适配将具有非常重要的作用。（相机将直接知道所有的物体的包围盒） 对于查看来说，其包围盒是唯一确定的。直接计算所有的mesh的总包围盒 对于爆炸来说，需要计算每一个零部件的包围盒，然后按照相应的爆炸函数进行爆炸。 对于建模来说，随着进行建模的过程...
基于主成分分析的有向包围盒计算【OBB】
新缸中之脑
07-04 1588
包围体（Bounding Volume）在视频游戏中有很多应用。在物理引擎中，包围体用于游戏对象之间的宽相相交测试，希望它们的（更简单的）测试减少花费在更复杂和更准确的相交测试上的周期数。在图形引擎中，边界体积根据相机的视锥体进行测试 - 相机视图之外的对象，随后被剔除并且不被绘制，从而节省宝贵的时间并保持可播放的帧速率。在使用的各种包围体中，我们将重点关注有向包围盒，简称 OBB，即Oriented Bounding Box。推荐：用快速搭建可编程3D场景。
碰撞测试1
08-08
在3D空间中，有六个面：前后、左右、上下，每个面都是一个二维的平面。 对于碰撞测试1的描述，其核心在于如何检查Ray与YZ平面的交点。这里我们首先要关注Ray在X轴上的位置。当Ray与YZ平面相交时，我们可以找到一个...
3D数学基础 图形与游戏开发
11-11
在游戏开发中，还会讲解空间分割技术，如边界盒（AABB）和包围球（OBB）用于提高碰撞检测效率，以及四叉树和Octree等空间分区数据结构。同时，图形优化策略，如LOD（细节级别）和烘焙光照等也会有所涉及。 最后，书...
计算机图形学-131
08-08
通过包围盒，我们可以快速地排除那些不可能与光线相交的物体，大大减少了计算量。判断光线是否与包围盒相交，需要计算光线穿过各个对面的时间，只有当光线进入所有对面的交集时，才表示光线进入了包围盒。 总结来说...
8-4-重积分的应用1
08-03
对于第二题，我们需要找到球面2222Rzyx=++4Rz和2Rz =所夹部分在平面上的投影，然后用三重积分来求解。球面在平面上的投影区域确定后，利用球坐标系进行积分，面积表示为：∫∫∫∫−−=′+′+=xyxyDDyxdyxRRdzzSσ...
勤行学区高一数学上学期第二次月考试题.doc
11-12
三视图则是从三个不同角度观察物体得到的平面图形，通过三视图可以了解物体的形状和尺寸。 5. **几何体的体积比和切削问题** - 题目中出现了切割几何体的问题，如零件的切削，要求学生掌握体积的计算方法，并理解...
矩形旋转碰撞,OBB方向包围盒算法实现
88zombie88的专栏
09-27 3924
转载注明：http://www.scottcgi.com/?p=121 如何进行2D旋转矩形的碰撞检测，可以使用一种叫OBB的检测算法(Oriented bounding box)方向包围盒。这个算法是基于SAT(Separating Axis Theorem)分离轴定律的。而OBB不仅仅是计算矩形的碰撞检测，而是一种算法模型。简单解释一下概念，包围盒和分离轴定律。 包围盒：是根据物体的集合形
包围盒算法基本知识（一）
qq_43718566的博客
04-24 2761
碰撞检测 定义：基于物体的空间位姿，确定两个或多个物体是否在一个或多个点发生接触 包围盒 定义 一种求解离散点集最优包围空间的方法。 基本思想 利用体积稍大的多边形近似代替研究对象 常见算法: 轴对齐包围盒AABB，包围球，方向包围盒OBB,固定方向凸包FDS AABB 基本思想： 采用一个长方体将物体包裹起来，进行两个物体的相交性检测时仅检测物体对应包围盒（包裹物体的长方体）的相交性 特点 对应的长方体每一个面都是与某个坐标轴平面平行 基本算法 xmin、xmax、ymin、ymax、zmin、zm
OCCT使用指南：Modeling Data
最新发布
weixin_44897632的博客
11-26 684
Modeling Data” 提供了数据结构来代表2D和3D几何模型。本手册将解释如何使用“Modeling Data”。从一组点创建OBB:该算法在 Thomas Larsson 和 Linus Källberg 的“快速计算紧密拟合的边界框”中进行了描述（FastOBBs.pdf）。它包括以下步骤：从点集创建最优OBB，为了从一组点创建最佳OBB，使用了与上述相同的算法，但在逻辑上进行了一些简化并增加了计算时间。对于最佳OBB，有必要检查所有可能由极值点创建的轴。
OpenCV学习(32) 求轮廓的包围盒
weixin_34259559的博客
11-16 647
      在OpenCV中，能够很方便的求轮廓包围盒。包括矩形，圆形，椭圆形以及倾斜的矩形(包围面积最小)集中包围盒。用到的四个函数是： Rect boundingRect(InputArray points) void minEnclosingCircle(InputArray points, Point2f&amp; center, float&amp; radius) Rotated...
计算机图形学复习笔记（问题回顾）
七七的博客
06-08 1924
合肥工业大学计算机图形学考前复习问题收集
计算几何之求取三维维点集的凸包/ 凸多边形包围盒(2D、3D点集)
z444_579的博客
10-23 3066
QuickHull(二维、三维空间点专用) // 方法思路：寻找一个凸核，并慢慢扩张成一个凸体包围盒; 1. 寻找各个方向上极值点 (2D则是4个，3D则是6个) 构成凸核(注意这个凸核一定包含于最终的凸包); 2. 删除凸核内的点集; 3. 若剩余点集非空，寻找各边(3D是各面) 的最远点加入凸包(注意必定是凸包上的点); 若点集为空，结束，返回凸包; 4. 构成新的凸核，转STEP2
VTK笔记——有向包围盒（OBB）
Andy的专栏
06-05 8871
什么是有向包围盒 《VTK笔记——轴对齐包围盒（AABB）》提到了有向包围盒(OBB/Oriented bounding box)。 有向包围盒，是较为常用的包围盒类型。简单的说，它就是包围模型的最小盒子，它是根据模型的几何形状来决定包围盒的大小和方向，无需和坐标轴垂直，方向是任意的，这是它的特点，但也导致了它生成的复杂性。 如何获取有向包围盒 在VTK中，有一个算法类vtkOBBTree可以得到...
OBB盒的实现
ailuyan0834的博客
11-15 1331
制造几何仿真中的碰撞检测通常视为针对刚体对象间的碰撞检测，这样的话可以把非刚体即软体的建模和变形算法对碰撞检测的影响减少到最小。常见成熟的基于包围盒的碰撞检测（box intersection test）算法如： 1）沿坐标轴的包围盒AABB（axis-aligned bounding boxes）在碰撞检测的研究历史中使用得最久最广, 一个给定对象的AABB被定义为包含该对象且各边平...
pythonocc求解最小包围盒
04-07
PythonOCC使用Bnd_Box类来表示包围盒，可以使用BRepBndLib包中的函数计算BRep对象的包围盒。以下是一个示例代码： ```python import OCC.Core.Bnd import OCC.Core.BRepBndLib import OCC.Core.TopoDS # 创建一个球体 sphere = OCC.Core.BRepPrimAPI.BRepPrimAPI_MakeSphere(10).Shape() # 计算球体的包围盒 bbox = OCC.Core.Bnd.Bnd_Box() OCC.Core.BRepBndLib.brepbndlib_Add(sphere, bbox) xmin, ymin, zmin, xmax, ymax, zmax = bbox.Get() print('Bounding box dimensions: ', xmax-xmin, ymax-ymin, zmax-zmin) ``` 输出： ``` Bounding box dimensions: 20.0 20.0 20.0 ``` 这里我们创建了一个半径为10的球体，并计算了球体的包围盒。最后输出了包围盒的尺寸。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

给算法爸爸上香

你的鼓励将是我创作的最大动力

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

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

打赏作者

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

抵扣说明：

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

余额充值