来自群友的分享

我是来自某大学本科，刚打完一个关于机器人的比赛，简单来说我在里面是负责识别一排矩形物体，返回最近的一个长方体并返回其相对于深度相机的三维坐标和角度。因为要使机器人运动，所以相对于机器人的角度信息也是必要的。

例如虚线框是我的画面，我就返回画面中最靠近中间的一个长方体，即下图中大概的红点位置。

我所提取的信息是x、z、angle。x就是左右，z就是距离。angle是相对于深度相机，若长方体的正面是垂直于相机就是0°，偏左是负数，偏右是正数。

好了，说完我要干的东西，接下来大概说一下我实现这个过程的代码思路。因为这排长方体相对于我深度相机的高度是不变的，所以先直接用passthough滤波。只滤剩中间一块地方，这一步已经能使得点云数据减少了很多，而且不会有太多其余的干扰。

pcl::PassThrough<PointT> pass; //直通滤波对象

pass.setInputCloud(cloud);

pass.setFilterFieldName("y");

pass.setFilterLimits(y_min_getammo,y_max_getammo);

pass.filter(*cloud_viewer);

进行下采样。

pcl::VoxelGrid<PointT> sor;

sor.setInputCloud(cloud_viewer);

sor.setLeafSize(0.003f, 0.003f, 0.003f);

sor.filter(*cloud_filtered);

然后我使用区域增长进行聚类分割，因为其实这里有一个难点是在于如何才能返回正面的中心，而不受长方体两边的面而影响。因为两边的面在不同的角度，采样获得的是不同的大小的点云，所以应该尽可能排除，而去分割出正面的那个面再去获得三维信息。

这部分是区域增长的代码。

pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);

pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;

normal_estimator.setSearchMethod(tree);

normal_estimator.setInputCloud(cloud_filtered);

normal_estimator.setKSearch(5);

normal_estimator.compute(*normals);

pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;

std::vector<pcl::PointIndices> cluster_indices;

reg.setMinClusterSize(900);

reg.setMaxClusterSize(2500);

reg.setSearchMethod(tree);

reg.setNumberOfNeighbours(10);

reg.setInputCloud(cloud_filtered);

reg.setInputNormals(normals);

reg.setSmoothnessThreshold(11 / 180.0 * M_PI);

reg.setCurvatureThreshold(0.5);

reg.extract(cluster_indices);

在这里说一下，要把正面的面提取得比较好的话，需要去调整一下这几个参数：

根据下采样sor的leafsize调整normal_estimator.setKSearch(5)和reg.setNumberOfNeighbours(10)，简单来说就是下采样后点越多，后两个的参数可以越大。

根据面与面之间的角度调整reg.setSmoothnessThreshold(11 / 180.0 * M_PI)和reg.setCurvatureThreshold(0.5)，这两个参数的意义去看看官网和源码的解释比较好。我这里是两个面互相呈90°，我调整出来这几个参数比较适合我自己对时间速度和精度的要求，我对速度的要求比较高，所以这里的参数还不是精度最好的参数。

接下来是根据分割后的聚类进行提取信息。这部分代码比较乱，最主要的框架是和官网上关于欧式聚类后续聚类提取相似，若觉得看不懂建议先看官网上的代码再看。

pcl::PointCloud<PointT>::Ptr all_cloud_cluster(new pcl::PointCloud<PointT>);

all_cloud_cluster->resize(cloud_filtered->size());

for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)

{

pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);

for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)

cloud_cluster->points.push_back(cloud_filtered->points[*pit]);

cloud_cluster->width = cloud_cluster->points.size();

cloud_cluster->height = 1;

cloud_cluster->is_dense = true;

//以下是对每个分割出来的聚类用包围盒算法提取三维信息。这部分的代码也是可以参考官网上关于包围盒的部分，也是建议先看过这部分再来看下面的代码比较能理解。http://pointclouds.org/documentation/tutorials/moment_of_inertia.php#moment-of-inertia

//我需要提取x、z、angle三个数据，x就只选择用((max_point_AABB.x + min_point_AABB.x) / 2)来表示就足够

z也是同样的道理，我只选用min_point_AABB.z，读者觉得有必要更高精度的z可以用其他方法进行更好的处理。

而关键就在于如何提取angle信息。

pcl::MomentOfInertiaEstimation <PointT> feature_extractor;

feature_extractor.setInputCloud(cloud_cluster);

feature_extractor.compute();

pcl::PointXYZ min_point_AABB;

pcl::PointXYZ max_point_AABB;

Eigen::Vector3f major_vector, middle_vector, minor_vector;

Eigen::Vector3f mass_center;

feature_extractor.getAABB(min_point_AABB, max_point_AABB);

feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);

feature_extractor.getMassCenter(mass_center);

//以下部分是计算angle信息，z_angle是OBB提取的角度，real_angle是AABB提取的角度，都是简单的相似直角三角形方法提取出来的。经过我自己的尝试发现直接用OBB的角度误差很大，而AABB的角度会更符合实际。但是因为偏左或偏右同一个角度，AABB提取出来的包围盒信息是一样的，所以我在此处是用OBB提取的z_angle判断正负方向，用AABB的real_angle当角度的绝对值。

pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

float z_angle, real_angle,lenght;

z_angle = atan2(z_axis.z, z_axis.x) * 180 / 3.14;

real_angle = atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) * 180 / 3.14;

if (z_angle>0) z_angle -= 90;

else z_angle += 90;

if (z_angle < 0) real_angle = (-real_angle);

//lenght是聚类的宽度，也是用简单的勾股定理获得，然后进行筛选。

lenght = sqrt((max_point_AABB.z - min_point_AABB.z)*(max_point_AABB.z - min_point_AABB.z) + (max_point_AABB.x - min_point_AABB.x)*(max_point_AABB.x - min_point_AABB.x));

if (lenght > 0.15 && lenght < 0.27)

{

cout << " z_angle,lenght:" << real_angle <<" "<<lenght<< endl;

all_cloud_cluster->operator+=(*cloud_cluster);

pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2);

pre_data[times][1] = min_point_AABB.z;

pre_data[times][2] = real_angle;

times++;

}

}

到此处，基本算法就已经完成了，后续的就是遍历pre_data，返回更靠中心、更受信赖的一组数据进行返回。其实总的来说是很简单的几个算法组合，因为要考虑实时性，所以也没有加上平滑点云表面等等的算法，这使得如果深度相机设备精度差、环境差，就会让处理后的数据差而达不到要求。所以其实此处的权衡要自己掌握，而到最后其实我感觉还是需要去更好的平滑一下点云数据再进行数据处理会更好，但在准备比赛期间已经没有时间去尝试了。我这里最后的速度大概能到40ms以内一帧。

还有另一个方法，是我只用欧式聚类分割，然后再进行平面模型拟合，从而根据平面返回的4个参数获得angle和z，x再用计算质心的数据获得。这个方法的数据会更精确，时间上和以上的方法差不多，甚至可能更快更准。