Better way of converting sensor_msgs::PointCloud2 to PointCloud<PointXYZRGB>
Hi,
I am receiving sensor_msgs::PointCloud2
inside a callback function. I am converting it to PointCloud<PointXYZRGB>
using following procedure-
- First convert
sensor_msgs::PointCloud2
topcl::PCLPointCloud2
- Later convert
pcl::PCLPointCloud2
topcl::PointCloud<pcl::PointXYZRGBA>
- Finally convert
pcl::PointCloud<pcl::PointXYZRGBA>
topcl::PointCloud<pcl::PointXYZRGB>
See below the code snippet-
inline void PointCloudXYZRGBAtoXYZRGB(pcl::PointCloud<pcl::PointXYZRGBA>& in, pcl::PointCloud<pcl::PointXYZRGB>& out)
{
out.width = in.width;
out.height = in.height;
out.points.resize(in.points.size());
for (size_t i = 0; i < in.points.size (); i++)
{
out.points[i].x = in.points[i].x;
out.points[i].y = in.points[i].y;
out.points[i].z = in.points[i].z;
out.points[i].r = in.points[i].r;
out.points[i].g = in.points[i].g;
out.points[i].b = in.points[i].b;
}
}
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGBA> temp_cloud;
pcl_conversions::toPCL(*msg, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, temp_cloud);
PointCloudXYZRGBAtoXYZRGB(temp_cloud, *cloud);
}
I am wondering if there exists better way which is efficient in terms of time taken. Kindly suggest.