Added squared norm to octomath::Vector3
Merge branch 'devel' of github.com:OctoMap/octomap into devel