Initial normalized_bearing implementation, needs some unit tests.
Calculate heading vector.
Decrease feature point density.