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

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

改进OBB包围盒

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

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

#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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353

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

最小投影面积包围盒

具体做法如下：

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

#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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343

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

相关阅读:
鹅长微服务发现与治理巨作PolarisMesh实践-上
 嵌入式学习笔记（61）位操作符
 TorchServe搭建codeBERT分类模型服务
 线上环境内存溢出-OutOfMemoryError
jenkins目录下的vue3项目——pnpm install后运行报错——奇葩问题解决
 2023版IDEA的下载、安装、配置、快捷键、模板、插件与使用
 Linux系统中线程同步方式中的条件变量操作方法
 ch4-2 音频信号的时域特征
 媒体查询技术
 windows10通过coursier安装scala
原文地址：https://blog.csdn.net/taifyang/article/details/131144254