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

news2024/11/29 7:40:51

算法流程：
（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;
}

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1052229.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

C++_基础语法

C++_基础语法

一、关键字 常用语法 #include<iostream>using namespace std;// 全局常量 #define DAY 30void main() {/** 变量与输出*/// 局部常量const int year 2023;// 控制台输出cout << "hello world" << endl;cout << "天&#xff1a;"…
阅读更多...
国庆加速度！新增功能点锁定功能，敏捷开发新增估算功能，助力项目快速突破！

国庆加速度！新增功能点锁定功能，敏捷开发新增估算功能，助力项目快速突破！

大家好&#xff0c;CoCode开发云旗下Co-Project V3.6智能项目管理平台正式发布&#xff0c;平台新增功能点锁定功能、敏捷开发模式新增估算板块和两种估算方式。 功能点锁定功能进一步提高了项目估算的灵活性和准确性&#xff0c;有利于提高项目估算效率&#xff1b;而敏捷开发…
阅读更多...
Unity2023打包首包从78Mb到3.0Mb-震惊-我做对了什么

Unity2023打包首包从78Mb到3.0Mb-震惊-我做对了什么

&#xff08;全程并没有使用AssetBundle , 历史原因&#xff0c;Resources目录还有不少资源残留&#xff09; 曾经的我在2019打包过最小包10m左右&#xff0c;后来发现到了Unity2020之后暴增到40m&#xff0c;又加上2023版本URP&#xff0c;1个Unity输出包可能至少55M 如下图…
阅读更多...
Ubuntu基于Docker快速配置GDAL的Python、C++环境

Ubuntu基于Docker快速配置GDAL的Python、C++环境

本文介绍在Linux的Ubuntu操作系统中&#xff0c;基于Docker快速配置Python、C等不同编程语言均可用的地理数据处理库GDAL的方法。 首先&#xff0c;我们访问GDAL库的Docker镜像官方网站&#xff08;https://github.com/OSGeo/gdal/tree/master/docker&#xff09;。其中&#x…
阅读更多...
Abstract Factory 抽象工厂模式简介与 C# 示例【创建型】

Abstract Factory 抽象工厂模式简介与 C# 示例【创建型】

〇、简介 1、什么是抽象工厂模式&#xff1f; 一句话解释&#xff1a; 提供一个接口&#xff0c;以创建一系列相关或相互依赖的抽象对象&#xff0c;而无需指定它们具体的类。&#xff08;将一系列抽象类装进接口&#xff0c;一次接口实现&#xff0c;就必须实例化这一系列抽象…
阅读更多...
如何在Ubuntu中切换root用户和普通用户

如何在Ubuntu中切换root用户和普通用户

问题 大家在新装Ubuntu之后&#xff0c;有没有发现自己进入不了root用户&#xff0c;su root后输入密码根本进入不了&#xff0c;这怎么回事呢&#xff1f; 打开Ubuntu命令终端&#xff1b; 输入命令&#xff1a;su root&#xff1b; 回车提示输入密码&#xff1b; 提示&…
阅读更多...
[ubuntu]ubuntu设置虚拟内存

[ubuntu]ubuntu设置虚拟内存

首先查看自己是否加过虚拟内存或者查看虚拟内存当前状态可以命令&#xff1a; free -mh 创建交换分区&#xff1a; sudo mkdir /swap cd /swap sudo dd if/dev/zero ofswapfile bs1024 count12582912 其中count是自己分配内存大小&#xff0c;上面为12GB&#xff0c;你可…
阅读更多...
视频异常检测：Human Kinematics-inspired Skeleton-based Video Anomaly Detection

视频异常检测：Human Kinematics-inspired Skeleton-based Video Anomaly Detection

论文作者&#xff1a;Jian Xiao,Tianyuan Liu,Genlin Ji 作者单位&#xff1a;Nanjing Normal University;The Hong Kong Polytechnic University 论文链接&#xff1a;http://arxiv.org/abs/2309.15662v1 内容简介&#xff1a; 1&#xff09;方向&#xff1a;视频异常检测…
阅读更多...
Opengl之混合

Opengl之混合

OpenGL中，混合(Blending)通常是实现物体透明度(Transparency)的一种技术。透明就是说一个物体（或者其中的一部分）不是纯色(Solid Color)的，它的颜色是物体本身的颜色和它背后其它物体的颜色的不同强度结合。一个有色玻璃窗是一个透明的物体，玻璃有它自己的颜色，但它最终的…
阅读更多...
rtp流广播吸顶喇叭网络有源吸顶喇叭

rtp流广播吸顶喇叭网络有源吸顶喇叭

SIP-7043 rtp流广播吸顶喇叭网络有源吸顶喇叭 一、描述 SIP-7043是我司的一款SIP网络有源吸顶喇叭&#xff0c;具有10/100M以太网接口&#xff0c;内置有一个高品质扬声器&#xff0c;将网络音源通过自带的功放和喇叭输出播放&#xff0c;可达到功率20W。SIP-7043作为SIP系统的…
阅读更多...
【分布式云储存】Springboot微服务接入MinIO实现文件服务

【分布式云储存】Springboot微服务接入MinIO实现文件服务

文章目录 前言技术回顾准备工作申请accessKey\secretKey创建数据存储桶公共资源直接访问测试 接入springboot实现文件服务依赖引入配置文件MinIO配置MinIO工具类 OkHttpSSLSocketClient兼容ssl静态资源预览解决方案资源上传预览测试测试结果 前言 上篇博客我们介绍了分布式云存…
阅读更多...
四川玖璨电子商务有限公司提供专业的抖音培训课程

四川玖璨电子商务有限公司提供专业的抖音培训课程

在今天的数字时代&#xff0c;抖音等社交媒体平台已经成为企业和个人扩大影响力、提高品牌知名度的主要战场。然而&#xff0c;如何在繁杂的社交媒体市场中取得成功呢&#xff1f;四川玖璨电子商务有限公司为你提供了一个解决方案——高质量的抖音培训课程。 四川玖璨电子商务…
阅读更多...
黑马程序员Docker快速入门到项目部署（学习笔记）

黑马程序员Docker快速入门到项目部署（学习笔记）

目录 一、Docker简介 二、安装Docker 2.1、卸载旧版 2.2、配置Docker的yum库 2.3、安装Docker 2.4、启动和校验 2.5、配置镜像加速 2.5.1、注册阿里云账号 2.5.2、开通镜像服务 2.5.3、配置镜像加速 三、快速入门 3.1、部署MYSQL 3.2、命令解读 四、Docker基础 …
阅读更多...
PostMan的学习

PostMan的学习

PostMan的学习 目录 环境变量和全局变量接口关联内置动态参数以及自定义动态参数实现业务闭环Postman断言批量运行collection数据驱动之CSV文件和JSON文件测试必须带请求头的接口Mock Serviers 服务器Cookie鉴权NewmanPostManNewManjenkins实现接口测试持续集成 参考资料&am…
阅读更多...
在线文档生成：Swagger

在线文档生成：Swagger

1 简介 现在的项目开发都是前后端分离&#xff0c;前端和后端是两拨人在开发&#xff0c;所以这就涉及到前后端人员的接口交互了。如果使用自己维护的接口文档或口口相传的话&#xff0c;很容易出现接口更新不及时的问题。这个时候就需要像Swagger这样的在线文档生成框架出马了…
阅读更多...
300元左右的耳机哪个性价比最好、好用的开放式耳机推荐

300元左右的耳机哪个性价比最好、好用的开放式耳机推荐

作为经常使用蓝牙耳机的小伙伴们而言&#xff0c;入耳式耳机戴久了会带来不舒适的感觉&#xff0c;开放式耳机显然是最舒服的选择&#xff0c;不入耳不会产生异物感&#xff0c;在户外运动可以时刻保持与外界连接更安全&#xff0c;也不会因为耳塞的卫生问题造成耳道感染&#…
阅读更多...
嵌入式学习笔记（35）外部中断

嵌入式学习笔记（35）外部中断

6.9.1什么是外部中断 (1)内部中断就是指中断源来自于SoC内部&#xff08;一般是内部外设&#xff09;&#xff0c;譬如串口、定时器等部件产生的中断&#xff1b;外部中断是SoC外部的设备&#xff0c;通过外部中断对应的GPIO引脚产生的中断。 (2)按键在SoC中就使用了外部中断…
阅读更多...
【MySQL】数据类型（二）

【MySQL】数据类型（二）

文章目录 一. char字符串类型二. varchar字符串类型2.1 char和varchar比较 三. 日期和时间类型四. enum和set类型4.1 set的查询 结束语 一. char字符串类型 char (L) 固定长度字符串 L是可以存储的长度&#xff0c;单位是字符&#xff0c;最大长度是255 MySQL中的字符&#xff…
阅读更多...
No159.精选前端面试题，享受每天的挑战和学习

No159.精选前端面试题，享受每天的挑战和学习

🤍 前端开发工程师（主业）、技术博主（副业）、已过CET6 🍨 阿珊和她的猫_CSDN个人主页 🕠 牛客高级专题作者、在牛客打造高质量专栏《前端面试必备》 🍚 蓝桥云课签约作者、已在蓝桥云课上架的前后端实战课程《Vue.js 和 Egg.js 开发企业级健康管理项目》、《带你从入…
阅读更多...
Kafka（一）使用Docker Compose安装单机Kafka以及Kafka UI

Kafka（一）使用Docker Compose安装单机Kafka以及Kafka UI

文章目录 Kafka中涉及到的术语Kafka镜像选择Kafka UI镜像选择Docker Compose文件Kafka配置项说明KRaft vs Zookeeper和KRaft有关的配置关于Controller和Broker的概念解释Listener的各种配置 Kafka UI配置项说明 测试Kafka集群Docker Compose示例配置 Kafka中涉及到的术语 对于…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023