read_pcd.py

import os
import re
import sys
import copy
import struct
import pprint
import traceback

def pcd_read(filename, verbose=0, verbose_steps=0):
    # Refer this to unpack - https://docs.python.org/3/library/struct.html#format-characters
    with open(filename, 'rb') as fp:
        data_ip      = fp.read()
        data_op      = {}
        data_header  = ''
        data_payload = ''
        
        matchObj = re.finditer(b'[\n]DATA\s(\S*)[\n]', data_ip, flags = 0)
        for group in matchObj:
            ## Step1 : Get the header
            data_header   = data_ip[:group.end(0)]
            data_header   = data_header.decode('utf-8').split('\n')
            field_names   = data_header[2].split(' ')[1:]
            field_size    = data_header[3].split(' ')[1:]
            field_type    = data_header[4].split(' ')[1:]
            data_count    = int(data_header[9].split(' ')[1])
            viewpoint     = data_header[8] 
            data_type     = data_header[10].split(' ')[1].lower()
            
            if verbose:
                print (' - field_names : ', field_names)
                print (' - field_size  : ', field_size)
                print (' - field_type  : ', field_type)
                print (' - data_count  : ', data_count, ' rows ')
                print (' - viewpoint   : ', viewpoint)
                print (' - data_type   : ', data_type)
            
            ## Step1.1 : Acumulate data on fields --> fields_dict
            fields_dict   = {}
            for i, field_name in enumerate(field_names):
                if int(field_size[i]) == 8:
                    fields_dict[i] = [str(field_name), int(field_size[i]), 'd']
                elif int(field_size[i]) == 2:
                    fields_dict[i] = [str(field_name), int(field_size[i]), 'H']
                else:
                    fields_dict[i] = [str(field_name), int(field_size[i]), str(field_type[i]).lower()] 
            if verbose : 
                print ('\n ----- Fields -----')
                pprint.pprint(fields_dict, indent=4)
                
            ## Step2 - Iterate through the payload
            data_payload   = data_ip[group.end(0):]
            data_count_tmp = 0
            i              = 0
            
            for id_ in fields_dict:
                data_op[fields_dict[id_][0]] = []
            if verbose : 
                print ('\n ----- Output Skeleton -----')
                pprint.pprint(data_op)
            
            if data_type == 'ascii':
                data_payload = data_payload.decode('utf-8')
                rows = data_payload.split('\n')
                while (i < len(rows)):
                    row_vals = rows[i].split(' ')
                    for id_ in fields_dict:
                        field_name = str(fields_dict[id_][0])
                        field_size = int(fields_dict[id_][1])
                        field_type = str(fields_dict[id_][2])
                        if field_type in ['f', 'd']:
                            tmp_data = float(row_vals[id_])
                        elif field_type == 'H':
                            tmp_data = int(row_vals[id_])
                        data_op[field_name].append(tmp_data)
                        
                    i += 1
                    data_count_tmp += 1
                    if data_count_tmp == data_count:
                        break
            
            elif data_type == 'binary':
                while(i < len(data_payload)):
                    try:
                        for id_ in fields_dict:
                            field_name = str(fields_dict[id_][0])
                            field_size = int(fields_dict[id_][1])
                            field_type = str(fields_dict[id_][2])
                            tmp_data   = data_payload[i:i+field_size]

                            if len(tmp_data) == field_size:
                                tmp_data   = struct.unpack(field_type, tmp_data)[0]
                            elif len(tmp_data) == 0:
                                break
                            if verbose_steps : 
                                print ('data_count_tmp : ',data_count_tmp+1, ' || i : ', i, ' || field : ', field_name,' || data : ', tmp_data)

                            i  += field_size
                            data_op[field_name].append(tmp_data)

                        data_count_tmp += 1

                        if data_count_tmp == data_count:
                            break

                    except Exception as e:
                        print (' - Error : ', e)
                        traceback.print_exc()
                        sys.exit(1)
        
        if verbose:
            for id_ in fields_dict:
                field_name = str(fields_dict[id_][0])
                print ('\n ----- Output Count -----')
                print (' - Field : ', field_name, ' || Data Count : ', len(data_op[field_name]))
                break
        return data_op, fields_dict

def pcd_write_bin(data_op, fields_dict, data_op_file, fields_required=[],verbose=0):
    data = []
    field_names = []
    
    if len(fields_required):
        for id_ in fields_dict:
            field_name = fields_dict[id_][0]
            if field_name not in fields_required:
                data_op.pop(field_name)
            else:
                field_names.append(field_name)
    
    else:
        field_names = [fields_dict[id_][0] for id_ in fields_dict]
    
    ## Step2 - binary data
    print ('\n ----- Writing .bin file -----')
    print (' - Writing these field_names : ', field_names)
    for i in range(len(data_op[field_names[0]])): # scroll down the rows
        for field_name in field_names:
            data.append(data_op[field_name][i])
    
    payload = bytes()
    payload = payload.join((struct.pack('f', val) for val in data))
    
    print (' - Writing to ', data_op_file)
    with open(data_op_file, 'wb') as fp:
        fp.write(payload)
              
 if __name__ == "__main__":
    data_pcds  = ['data/1523643737.998116000.pcd']
    for data_pcd in data_pcds:
        data_op, fields_dict = pcd_read(data_pcd, verbose=1)
        data_op_file         = data_pcd.split('.pcd')[0] + '_modified.bin' 
        pcd_write_bin(data_op, fields_dict, data_op_file, fields_required=['x', 'y', 'z', 'intensity'],verbose=1)
        
        if 0:
            ## Cross Check Data : 
            rows_count = 4
            print ('\n ----- Original Data ----- : ', len(data_op['x']))
            for i in range(rows_count):
                print (data_op['x'][i], data_op['y'][i], data_op['z'][i], data_op['intensity'][i])
            
            data_bin = np.fromfile(data_op_file, dtype=np.float32)
            print ('\n ----- .bin data ----- : ', len(data_bin))
            print (' - ',  data_bin[:rows_count * rows_count])

read_pcd_files.py

import re
from struct import *

def read_pcd(filename):
    with open(filename, 'rb') as fp:
        data         = fp.read()
        data_header  = ''
        data_payload = ''

        ## Step1 : Get the header
        matchObj = re.finditer(b'[\n]DATA\s(\S*)[\n]', data, flags = 0)
        for group in matchObj:
            data_header   = data[:group.end(0)]
            data_header   = data_header.decode('utf-8').split('\n')
            print (' - Header : \n', data_header) ##- Header : ['# .PCD v0.7 - Point Cloud Data file format', 'VERSION 0.7', 'FIELDS x y z rgba', 'SIZE 4 4 4 4', 'TYPE F F F U', 'COUNT 1 1 1 1', 'WIDTH 0', 'HEIGHT 0', 'VIEWPOINT 0 0 0 1 0 0 0', 'POINTS 212001', 'DATA binary', '']

            # Step2 - Iterate through the payload
            data_payload  = data[group.end(0):]
            prev_i = 0
            count = 0
            X,Y,Z,R,G,B, A = [], [], [], [], [], [], [] 
            for i in range(4,200,4):
                if count == 3:
                    count = -1
                    R.append(data_payload[i])
                    G.append(data_payload[i+1])
                    B.append(data_payload[i+2])
                    A.append(data_payload[i+3])
                else:
                    if count == 0 : X.append(unpack('f', data_payload[prev_i:i]))
                    if count == 1 : Y.append(unpack('f', data_payload[prev_i:i]))
                    if count == 2 : Z.append(unpack('f', data_payload[prev_i:i]))
                prev_i = i
                count += 1
                
    return data_header, [X,Y,Z,R,G,B,A]

if __name__ == "__main__":
    filename = '../boxes2.pcd'
    data_header, data_payload = read_pcd(filename)
  

visualize_lidar.py

"""
Topic            : Visualize the KiTTI Dataset (LiDAR)
References
 - LiDAR Dataset : www.cvlibs.net/datasets/kitti
 - Data Download : www.cvlibs.net/datasets/kitti/raw_data.php
 - Blog          : 

 - Notes
    - Tested on Python3.5
"""
import os
import h5py         # pip3 install h5py
import pykitti      # pip3 install pykitti
import vispy        # pip3 install vispy
import vispy.scene
import numpy as np


## 1. Convert multiple .bin files to  single .h5 format
def data_write_h5(url_hdf5, dataset, dataset_limit=1000):
    with h5py.File(url_hdf5, "w") as hf: 
        for i, each in enumerate(dataset.velo):
            if i % 10 == 0:
                print (' - Velo File : ', i+1)

            points = each[:,[0,1,2]]
            colors = []
            for pt in points:
                colors.append((1, 1, 1, 1))
            colors = np.array(colors)

            hf.create_dataset('points_{0}'.format('%.3d' % (i)), data=points)
            hf.create_dataset('colors_{0}'.format('%.3d' % (i)), data=colors)

            if i > dataset_limit:
                break

## 2. Plot data from a single .h5 file
def data_plot_h5(url_hdf5, point_size=3):
    
    ## Step1 - Read Data
    points = []
    colors = []
    with h5py.File(url_hdf5, "r") as hf:
        for name in hf:
            if 'points' in name:
                points.append(np.array(hf.get(name)))
            if 'colors' in name:
                colors.append(np.array(hf.get(name)))

    ## Step2 - Setup the canvas
    canvas = vispy.scene.SceneCanvas(keys='interactive', show=True)
    view = canvas.central_widget.add_view()
    view.bgcolor = '#111111'
    view.camera = ['perspective', 'panzoom', 'fly', 'arcball', 'base', 'turntable', None][2]
    if 1:
        view.camera.fov = 60
        view.camera.scale_factor = 0.7
        view.camera.keymap['Right'] = (1,5)
        view.camera.keymap['Left']  = (-1,5)
        view.camera.keymap['Up']    = (1,4)
        view.camera.keymap['Down']  = (-1,4)

    
    axis    = vispy.scene.visuals.XYZAxis(parent=view.scene)
    scatter = vispy.scene.visuals.Markers(parent=view.scene)
    canvas.show()

    ## Step3 - Update function
    def update(data):
        matrix, colors = data
        scatter.set_data(matrix
                        , edge_color=None, face_color=colors
                        , size=point_size)
        

    ## Step4 - Update canvas with first lidar frame
    update([points[0], colors[t]])    

    # Step5 - Handling key events
    @canvas.events.key_press.connect
    def keypress(e):
        global t
        print('  - File Index : ', t)
        if e._key.name == '=':
            t = min(t + 1, len(points) - 1)
            update([points[t], colors[t]])

        elif e._key.name == '-':
            t = max(t - 1, 0)
            update([points[t], colors[t]])
        
        else:
            pass
            # print (' - key : ', e._key.name)


if __name__ == "__main__":
    ## Define filenames
    url_hdf5 = 'lidar_kitti_plot.h5'
    data_write = 0

    ## First read the .bin files from a KITTI drive and convert to .h5 (data_write=1)
    if data_write : 
        DATA_DIR = os.path.abspath(os.path.join(os.getcwd(), '../data'))
        print (' - Data Dir : ', DATA_DIR)

        basedir = DATA_DIR
        date    = '2011_09_26'
        drive   = '0001'
        dataset = pykitti.raw(basedir, date, drive)#, frames=range(0, 50, 5))
        print ('\n - Read KITTI Dataset')
        data_write_h5(url_hdf5, dataset, dataset_limit=40)

    ## Then plot that 3D LiDAR data (data_write=0)
    ## You can use wasd and arrow keys to move around. (f,c)-> increase or decrease your z
    ## Note : Zooming does not currently work, so scrolling wont have an effect
    else:
        t = 0
        data_plot_h5(url_hdf5)
        vispy.app.run()
            
        

visualize_lidar_umich.py

"""
Topic : Visualize the UMichigan Ford Dataset
References
 - Umich Dataset : http://robots.engin.umich.edu/SoftwareData/Ford
 - Data Download : http://robots.engin.umich.edu/uploads/SoftwareData/Ford/dataset-1-subset.tgz
 - Blog          : 

 - Notes
    - Tested on Python3.5
"""

## Import Libraries
import os
import h5py          # pip3 install h5py
import vispy         # pip3 install vispy
import vispy.scene
import numpy as np
import scipy.io as sio

## 1. Read a matlab file
def get_data(filename_mat, verbose = 0):
    mat_contents = sio.loadmat(filename_mat)
    keys = mat_contents.keys()
    data = mat_contents['SCAN']
    a = data[0][0]

    if verbose:
        for each in a:
            print (each.shape)
    data = a[0].T
    return data

## 2. Read a directory containing matlab files and store as hfd5
def write_datum_h5(dirname_mat, filename_h5, file_count_max = 100):
    datum = []
    file_count = 0
    if os.path.exists(dirname_mat):
        with h5py.File(filename_h5, 'w') as hf:
            for file_id, file in enumerate(sorted(os.listdir(dirname_mat))):
                file_tmp = os.path.join(dirname_mat, file)
                print (' - File : ', file_tmp)
                data = get_data(file_tmp)
                hf.create_dataset('data_%.4d' % (file_id) , data=data)
                file_count += 1
                if file_count > file_count_max:
                    return 1
        return 1
    else:
        return 0

## 3. Read a h5d5 file
def read_datum_h5(filename_h5):
    datum = []
    with h5py.File(filename_h5, 'r') as hf:
        for node in hf:
            print (' ->', node)
            datum.append(np.array(hf.get(node)))
        return datum

## Plot U.Mich LiDAR data saved as .h5 file 
def plot_data(datum, point_size=3):

    canvas = vispy.scene.SceneCanvas(keys='interactive', show=True)
    view = canvas.central_widget.add_view()
    view.bgcolor = '#ffffff'
    view.bgcolor = '#111111'
    view.camera = ['perspective', 'panzoom', 'fly', 'arcball', 'base', 'turntable', None][2]
    if 1:
        view.camera.fov = 60
        view.camera.scale_factor = 0.7
        view.camera.keymap['Right'] = (1,5)
        view.camera.keymap['Left']  = (-1,5)
        view.camera.keymap['Up']    = (1,4)
        view.camera.keymap['Down']  = (-1,4)

    axis = vispy.scene.visuals.XYZAxis(parent=view.scene)
    scatter = vispy.scene.visuals.Markers(parent=view.scene)
    scatter.set_data(datum[0], size=point_size)

    def update(data):
        scatter.set_data(data, size=point_size)

    @canvas.events.key_press.connect
    def keypress(e):
        print (' - Event : ', e._key.name)
        global t
        print('  - File Index : ', t)
        if e._key.name == '=':
            t = min(t + 1, len(datum) - 1)
            update(datum[t])

        if e._key.name == '-':
            t = max(t - 1, 0)
            update(datum[t])

    canvas.show()

if __name__ == '__main__':
    ## Define the directory containing the .mat files
    dirname_umich_scans  = './plot_vispy'  #contains the .mat files
    filename_h5          = 'data_umich.h5'; file_count_max = 1000
    

    ## First convert all .mat files into a .h5 file (data_write=1)
    data_write = 1
    if data_write:
        write_datum_h5(dirname_umich_scans, filename_h5, file_count_max)

    ## Then plot that 3D LiDAR data (data_write=0)
    ## You can use wasd and arrow keys to move around. (f,c)-> increase or decrease your z
    ## Note : Zooming does not currently work, so scrolling wont have an effect
    else:
        t = 0
        datum_3d = read_datum_h5(filename_h5)    
        plot_data(datum_3d, point_size=2)
        vispy.app.run()