now using extractIndices built-in function
now clustering seems to work
Can now segment the two robots from two_robots.bag