Code restructure

Added .hpp files and moved templated class implementation from .h to .hpp files. Also added explicit template instantiations in the .cpp files. Removed templates from a couple of classes where templates weren't needed. Has been (briefly) tested on the robot. 
This PR builds on https://github.com/strands-project/strands_3d_mapping/pull/60, and that should be merged first.