Many changes:
- reorganization and cleanup of some code
- fixed a severe bug on handling LIDAR frame
- work in sensor model still in progress
- better integration with ROS environment (however, still using a bag for testing)
(note: resampling disabled for now for testing purposes)