Update ud_pcl_utils.cpp