almost competely ported over to ros
code currently does not run; requires conversion back to PointRGBA