Add unit tests using boost test

Added UnitTests for a few classes, as well as fixing some minor bugs and cleaning up a little bit in the process.
We are computing distances between two locations here and not between a location and a segment.