code rearrangement

still need to add the KITTI loading part & check a lot