Blocks are identified using the conditionAnd, PackedRGBComparison, ConditionalRemoval functions from the PCL library. Each block is identified by its color and we identify each color by specifying the RGB ranges of each color.

How to identify RGB ranges of the colored block in the point cloud?

In the rviz GUI window, use the selection option towards the top and select the points of a specific color, you can find the RGB values of the selected points in the selection panel, using these points manually compute the range of RGB values for each color.

This is implemented under the function named "extract_colors" of the script detect_objects.cpp . Adjust the RGB values if required.

Remove Outliers:

Outlier points are removed by using the RadiusOutlierRemoval function, which searches the cloud and removes the point's that has less than specified minimum neighbor points within the certain radius.

Block identification from colored Point Cloud:

For each colored point cloud, Iterate through the points and get the sides of the block (dx, dy, dz). dx is obtained by the difference between the point with lowest x co-ordinate value and the point with the highest x co-ordinate value, similarly for dy and dz. If sum of dx and dy is less than equal to the length of it's diagonal then it's a block.

Publish the location of the colored block by finding it's centroid.

Running the scripts for Demo