added debug output to show correspondences in rviz