Added additional diagnostics to the Plane data structure.
First we will check for horizontal vs non-horizontal planes (normal along y)
Then we will check for
More refactoring.
In multi_plane_segmentation.cpp, I not only find all pairs of planes that
have a common principal direction, but I capture them in a datastructure and
remove duplicates via averaging.