Table segmentation and object clustering can be done much better by fine-tuning the related parameters.
It is strange though, the cylinder model in gazebo seems to have problems with ray collisions. In some part of the video, cylinder is divided into two during clustering stage since rays directly pass through top of the cylinder.
The main loop seems to be really slow, but this is due to the 3d data acquisition rate, around 1-2 Hz. When the objects are moved, system reacts to the changes quickly, although the code hasn’t been optimized at all.
Swissranger sr4k model is put just across the robot for the time being, but it might be better, in terms of manipulation performance, putting the camera next to/top of the robot, or at least merging the sensori information obtained from the robots on-board 3d sensors. Yet, this is a temporary setup, and we don’t have that sort of sensors on our iCub robot, so I’m not going to deal with those issues.