求解平面上物体的有向3d包围盒
算法流程：
（1）点云下采样（体素滤波）；
（2）ransac算法分割拟合地面平面；
（3）裁剪工作区域（指定空间中四个点，裁剪点云只保留在（2）中平面上的投影在四边形内部的点）；
（4）再用ransac算法去除多余平面；
（5）Euclidean聚类算法分割出目标物体的点云簇；
（6）通过包围盒算法计算包围盒。
改进OBB包围盒
由于物体是放在地面上，因此可以利用地面平面的法向量约束物体包围盒的朝向。具体做法如下：
- 把物体点云投影到桌面平面上得到底面投影点云；
- 计算底面的obb外接矩形，得到物体包围盒的朝向从而建立局部坐标系；
- 将物体点云投影到局部坐标系z轴上得到投影轴线的点云；
- 投影得到点云的中心作为物体包围盒的中心；
- 通过物体包围盒的朝向和中心确定其包围盒。
#include ::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;
}
可视化结果：
最小投影面积包围盒
具体做法如下：
- 把物体点云投影到桌面平面上得到底面投影点云；
- 计算底面投影点云点集的最小外包旋转矩形，得到物体包围盒的朝向从而建立局部坐标系；
- 将物体点云投影到局部坐标系z轴上得到投影轴线的点云；
- 投影得到点云的中心作为物体包围盒的中心；
- 通过物体包围盒的朝向和中心确定其包围盒。
#include 可视化结果：
