Added option to train with groundtruth cost
Make3d init state
Delete unused nodes (need to verify)
Merge conflict