A system for generating a ground truth dataset for motion planning is disclosed. The system includes: a collecting module configured to collect LiDAR scans; a detecting module configured to detect two pose estimates that are closest to LiDAR scan accusation time; a determining module configured to determine LiDAR's poses based on an interpolation; and a transforming module configured to transform LiDAR raw scans into undistorted LiDAR scans.