Convert point cloud from pointcloud2 (rosbag) to bin (KITTI)

Question

How can I convert a point cloud saved in rosbag, in format sensor_msgs/PointCloud2, to .bin files in KITTI format?

I know that it is possible to convert to .pcd (http://wiki.ros.org/pcl_ros#pointcloud_to_pcd) so perhaps even a pcd to bin converter would be enough.

Is there any available tool to do this?

I've found this, but it needs ROS kinetic (legacy ROS version).

Answer 1

A python script to do it:

  pc = pypcd.PointCloud.from_msg(msg)
  x = pc.pc_data['x']
  y = pc.pc_data['y']
  z = pc.pc_data['z']
  intensity = pc.pc_data['intensity']
  arr = np.zeros(x.shape[0] + y.shape[0] + z.shape[0] + intensity.shape[0], dtype=np.float32)
  arr[::4] = x
  arr[1::4] = y
  arr[2::4] = z
  arr[3::4] = intensity
  arr.astype('float32').tofile('filename.bin')

Where x,y,z and intensity are arrays for a single point cloud. It's not strictly needed to use pypcd. (Source)

Also this conversion tool can actually be used without ROS, using another tool for the conversion to pcd file.

Answer 2

FOR ROS1 Noetic

use ros_numpy package (see imports)

After cloning ros_numpy from Github, fix line 244 of point_cloud2.py from:

def get_xyz_points(cloud_array, remove_nans=True, dtype=np.float):

to:

def get_xyz_points(cloud_array, remove_nans=True, dtype=np.float32):

Node for saving .bin from PointCloud2 topic (also saves a .pcd using the .bin file, so you can verify it is packaging correctly. Make sure to remove this after verifying so it can run faster.)

#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
from std_msgs.msg import Header
import sensor_msgs.point_cloud2 as pcl2
import numpy as np
import struct
import open3d as o3d

# Clone from https://github.com/eric-wieser/ros_numpy
import ros_numpy

def bin_to_pcd(binFileName):
    size_float = 4
    list_pcd = []
    with open(binFileName, "rb") as f:
        byte = f.read(size_float * 4)
        while byte:
            x, y, z, intensity = struct.unpack("ffff", byte)
            list_pcd.append([x, y, z])
            byte = f.read(size_float * 4)
    np_pcd = np.asarray(list_pcd)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(np_pcd)
    return pcd

def callback(data):
    rospy.loginfo('Received a PointCloud2 message')
    
    pc = ros_numpy.numpify(data)

    print(pc.shape)
    points=np.zeros((pc.shape[0],pc.shape[1],4))
  
    points[:,:,0]=pc['x']
    points[:,:,1]=pc['y']
    points[:,:,2]=pc['z']
    points[:,:,3]=pc['intensity']

    # Add to
    p = np.array(points, dtype=np.float32)

    global pc_number
    pc_number += 1

    # Save files
    bin_file = f'/home/user/Desktop/bin/{pc_number}.bin'       # TODO: Replace with your desired folder for .bin files
    pcd_file = f'/home/donceykong/Desktop/pcd/{pc_number}.pcd' # TODO: Replace with your desired folder for .pcd files

    p.astype('float32').tofile(bin_file)
    pcd = bin_to_pcd(bin_file)

    o3d.io.write_point_cloud(pcd_file, pcd)

def listener():
    global pub
    global pc_number
    
    # Init pc number
    pc_number = 0
    
    rospy.init_node('pointcloud_to_bin_file', anonymous=True)

    # Subscribe to the input PointCloud2 topic
    input_cloud = 'ouster/points' # TODO: Change this to your pc topic
    rospy.Subscriber(input_cloud, PointCloud2, callback)

    # Create a publisher for the output PointCloud2 topic
    pub = rospy.Publisher('/output_cloud', PointCloud2, queue_size=10)

    rospy.spin()

if __name__ == '__main__':
    listener()