rewrote pcd file creation in C for a big speedup