How do I convert a mesh to a pointcloud for use with pcl ICP
I want to find partial overlaps between meshes in the household object database and pointcloud sensor data. My method is to convert the arm_navigation_msgs::Shape to a pcl::PointCloud and then find clusters that overlap through pcl::IterativeClosestPoint. I have all pieces working, I just think my function for converting the shape meshes to pointclouds is inefficient. Is there a way to take advantage of the underlying structure to speed up the conversion?
This is my current implementation
pcl::PointCloud<pcl::PointXYZ> PCLMeshOverlap::convertMeshToPointcloud(arm_navigation_msgs::Shape mesh)
{
ROS_INFO("Converting mesh to pointcloud");
pcl::PointCloud<pcl::PointXYZ> cloud_out;
pcl::PointXYZ point;
for (int i = 0; i < mesh.vertices.size(); i++)
{
float isNew = true;
for (int j = 0; j < cloud_out.points.size(); j++)
{
if ( mesh.vertices.at(i).x == cloud_out.points.at(j).x &&
mesh.vertices.at(i).y == cloud_out.points.at(j).y &&
mesh.vertices.at(i).z == cloud_out.points.at(j).z )
{
isNew = false;
break;
}
}
if (isNew)
{
point.x = mesh.vertices.at(i).x;
point.y = mesh.vertices.at(i).y;
point.z = mesh.vertices.at(i).z;
cloud_out.points.push_back(point);
}
}
cloud_out.width = cloud_out.points.size();
cloud_out.height = 1;
cloud_out.is_dense = true;
cloud_out.header.frame_id = "/base_link";
return cloud_out;
}
*Edited because the original code had some typos