KITTI BIN파일을 PCD(Text) 파일로 변환하기(python)

1. 개요

KITTI 에서 제공하는 LADAR(Laser Detection And Ranging)  센서정보를 Visualize 하여 보자. 제공되는 bin 파일을 텍스트 PCD 파일로 변환하고 사용할 것이다. 아래 싸이트에서 다운로드를 한다.
http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d

 

2. bin 파일을 PCD 텍스트 파일로 변환한다.

import struct
import sys
from struct import unpack
def readBinFile(fname):
    x = []
    y = []
    z = []
    try:
        with open(fname, 'rb') as fp:
            while True:
                val1 = unpack('<1f', fp.read(4))
                val2 = unpack('<1f', fp.read(4))
                val3 = unpack('<1f', fp.read(4))
                val4 = unpack('<1f', fp.read(4))
                x.append(val1[0])
                y.append(val2[0])
                z.append(val3[0])
                print(val1[0], val2[0], val3[0])
    except Exception as e:
        print(e)
        pass
    finally:
        fp.close()  # 파일 닫기
    return x,y,z

def writePCDFile(fname,x,y,z):
    numPoints= len(x)
    with open(fname, 'w') as fp:
        fp.write("VERSION 0.7\n")
        fp.write("FIELDS x y z\n")
        fp.write("SIZE 4 4 4\n")
        fp.write("TYPE F F F\n")
        fp.write("WIDTH "+str(numPoints)+"\n")
        fp.write("HEIGHT 1\n")
        fp.write("POINTS "+str(numPoints)+"\n")
        fp.write("DATA ascii\n")
        for index in range(numPoints):
            txtLine = "{} {} {}\n".format(x[index],y[index],z[index] )
            fp.write(txtLine)
        pass

if __name__ == "__main__":
    fname1 ="../StereoDataset/000001.bin"
    fname2 ="../StereoDataset/000001.pcd"
    x,y,z =readBinFile(fname1)
    writePCDFile(fname2, x, y, z)
    pass

 

PCL 라이브러리를 사용하여 .pcd 파일을 Visualize를 하여본다,