I want to use [convert_pointcloud_to_image.cpp](https://github.com/ros-perception/perception_pcl/blob/kinetic-devel/pcl_ros/tools/convert_pointcloud_to_image.cpp) to convert an unorganized point cloud to a 2D image, but the function only accepts an organized point cloud.
rosrun pcl_ros convert_pointcloud_to_image input:=/unorganized_pc_object_topic output:=/image_from_pc_topic
Input point cloud is not organized, ignoring!
It seems like there should be a pcl function for such a common operation that I can use to convert the unorganized point cloud to an organized one. I would expect there to be a way to do something like this, or an existing C++ function where `camera_info` is used to organize the pointcloud:
rosrun pcl_ros convert_unorganized_to_organized input:=/unorganized_pc_object_topic cam_info:=/camera/color/camera_info output:=/organized_pc_topic
To clarify, I already understand conceptually what needs to happen, and I have been able to do this in Python, using the code provided below. I'm looking specifically for an existing node or exisiting `.cpp` function to do the equivalent.
points = pc2.read_points(msg_pointcloud, skip_nans=True, field_names=("x", "y", "z"))
camera_model.fromCameraInfo(intrinsics)
organized_points = []
for point in points:
p = [point[0], point[1], point[2]]
pp = camera_model.project3dToPixel(pc)
organized_points.append(pp)
organized_points = np.array(organized_points, dtype=int)
↧