moved code from cob_3d_segmentation