利用pcl点云库找到物体(鞋底)的上边缘
一、总体思路
1将点云按鞋底长度方向分成多个片段;2每个点云片段找到最高点;3将找到的最高点放到新的点云中显示
二、代码
for (float i = min.y; i < max.y; i=i+2.0)
{
for (size_t j = 0; j < cloud_final->points.size(); j++)
{
if (cloud_final->points[j].y >= i && cloud_final->points[j].y < i+2.0)
{
indexs.push_back(j);
}
}
pcl::copyPointCloud(*cloud_final, indexs, *cloud_final_subset);
pcl::getMinMax3D(*cloud_final_subset, min_subset, max_subset);
for (size_t k = 0; k < cloud_final_subset->size(); k++)
{
if (cloud_final_subset->points[k].x>=min_subset.x && cloud_final_subset->points[k].x < (max_subset.x + min_subset.x) / 2.0)
{
indexs_subset1.push_back(k);
}
else
{
indexs_subset2.push_back(k);
}
}
pcl::copyPointCloud(*cloud_final_subset, indexs_subset1, *cloud_subset1);
pcl::copyPointCloud(*cloud_final_subset, indexs_subset2, *cloud_subset2);
/*数据排序,找出最高点放到最后的点云结构中显示*/
subset1_maxz = cloud_subset1->points[0].z;
subset2_maxz = cloud_subset2->points[0].z;
index_maxz1 = 0;
index_maxz2 = 0;
for (int l = 0; l < cloud_subset1->points.size(); l++)
{
if (subset1_maxz< cloud_subset1->points[l].z)
{
subset1_maxz = cloud_subset1->points[l].z;
index_maxz1 = l;
}
}
cloud_topboundary->push_back(cloud_subset1->points[index_maxz1]);
for (int m = 0; m < cloud_subset2->points.size(); m++)
{
if (subset2_maxz < cloud_subset2->points[m].z)
{
subset2_maxz = cloud_subset2->points[m].z;
index_maxz2 = m;
}
}
cloud_topboundary->push_back(cloud_subset2->points[index_maxz2]);
indexs_subset.clear();
cloud_subset1->clear();
cloud_subset2->clear();
cloud_final_subset->clear();
indexs_subset1.clear();
indexs_subset2.clear();
indexs.clear();
}