Initial addition of boost graph structures and creation.

I think we can make this function take a vector of TrajectoryPt, as I believe everything it calls is generic to our base/derived class structure.