WIP on the optimizer. Already shows a mock-up cost function.

Writing the costmap class for the optimizer.