Introduction

This tutorial follows Tutorial: Live color segmentation using HSV color scale.

To run this tutorial you will need:

a Realsense camera like a D435 device
ViSP build with librealsense and PCL libraries as 3rd parties

We suppose here that you already set the HSV low/high ranges using the range tuner tool explained in Tutorial: HSV low/high range tuner tool.

Note that all the material (source code and images) described in this tutorial is part of ViSP source code (in tutorial/segmentation/color folder) and could be found in https://github.com/lagadic/visp/tree/master/tutorial/segmentation/color.

Point cloud segmentation

In Tutorial: Introduction to color segmentation using HSV color scale the pipeline to achieve the HSV color segmentation was to:

load or acquire an RGB image
convert from RGB to HSV channels using vpImageConvert::RGBaToHSV()
create a mask with HSV values that are in the low/high HSV ranges using vpImageTools::inRange()
using the mask and the RGB image create a segmented image using vpImageTools::inMask()

To extend this pipeline to create a point cloud that contains only the 3D points that are in the mask you need a RGB-D device. In our case we will consider a Realsense camera. The pipeline becomes:

configure the realsense grabber to acquire color and depth aligned images
int width = 848, height = 480, fps = 60;

vpRealSense2 rs;

rs2::config config;

config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);

config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);

config.disable_stream(RS2_STREAM_INFRARED, 1);

config.disable_stream(RS2_STREAM_INFRARED, 2);

rs2::align align_to(RS2_STREAM_COLOR);
get depth scale associated to the depth images and device intrisics
float depth_scale = rs.getDepthScale();

vpCameraParameters cam_depth = rs.getCameraParameters(RS2_STREAM_DEPTH,

vpCameraParameters::perspectiveProjWithoutDistortion);
grab color and depth aligned images
rs.acquire((unsigned char *)I.bitmap, (unsigned char *)(depth_raw.bitmap), NULL, NULL, &align_to);
convert from RGB to HSV channels using the vpHSV class along with vpImageConvert::convert() method, or vpImageConvert::RGBaToHSV() when the vpHSV class is not available
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

vpImageConvert::convert(I, Ihsv);

#else

vpImageConvert::RGBaToHSV(reinterpret_cast<unsigned char *>(I.bitmap),

reinterpret_cast<unsigned char *>(H.bitmap),

reinterpret_cast<unsigned char *>(S.bitmap),

reinterpret_cast<unsigned char *>(V.bitmap), I.getSize());

#endif
create a mask with pixels that are in the low/high HSV ranges using vpImageTools::inRange()
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

vpImageTools::inRange(Ihsv, hsv_values, mask);

#else

vpImageTools::inRange(reinterpret_cast<unsigned char *>(H.bitmap),

reinterpret_cast<unsigned char *>(S.bitmap),

reinterpret_cast<unsigned char *>(V.bitmap),

hsv_values,

reinterpret_cast<unsigned char *>(mask.bitmap),

mask.getSize());

#endif
create the point cloud object. Note here the Z_min and Z_max variables, which are used further in the code to remove 3D points that are too close or too far from the camera
float Z_min = 0.1;

float Z_max = 2.5;

pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
using the mask and the depth image update the point cloud using vpImageConvert::depthToPointCloud(). The corresponding point cloud is available in pointcloud variable.
vpImageConvert::depthToPointCloud(depth_raw, depth_scale, cam_depth, pointcloud, nullptr, &mask, Z_min, Z_max);
to know the size of the point cloud use
int pcl_size = pointcloud->size();

Instead, you can also use the value returned by vpImageConvert::depthToPointCloud() to get the point cloud size.

All these steps are implemented in tutorial-hsv-segmentation-pcl.cpp

To run this tutorial

$ cd $VISP_WS/visp-build/tutorial/segmentation/color
$ ./tutorial-hsv-segmentation-pcl --hsv-thresholds calib/hsv-thresholds.yml

In the next section we show how to display the point cloud.

Point could viewer

In tutorial-hsv-segmentation-pcl-viewer.cpp you will find an extension of the previous code with the introduction of vpDisplayPCL class that allows to visualize the point cloud in 3D with optionally additional texture information taken from the color image.

To add the point cloud viewer feature:

Fisrt you need to include vpDisplayPCL header
#include <visp3/gui/vpDisplayPCL.h>
Then, we declare two pcl objects for textured and untextured point clouds respectively
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());

pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
You can either run the point cloud viewer in a separate thread or in a the main thread (for instance for MacOS users, see vpDisplayPCL documentation). When running in a separate thread, to avoid concurrent access to pointcloud or pointcloud_color objects, we introduce a mutex, declare the PCL viewer object and launch the viewer thread, either for textured or untextured point clouds. When running in the main thread, we have to add the point clouds to the list of known point-clouds.
std::mutex pointcloud_mutex;

vpDisplayPCL pcl_viewer(opt_width, opt_height);

if (opt_mode == DisplayMode::THREADED) {

if (opt_pcl_textured) {

pcl_viewer.startThread(std::ref(pointcloud_mutex), pointcloud_color);

}

else {

pcl_viewer.startThread(std::ref(pointcloud_mutex), pointcloud);

}

}

else {

if (opt_pcl_textured) {

pcl_viewer.addPointCloud(std::ref(pointcloud_mutex), pointcloud_color);

}

else {

pcl_viewer.addPointCloud(std::ref(pointcloud_mutex), pointcloud);

}

}
In the while() loop we update the point cloud using the mutex shared with the pcl viewer
int pcl_size;

if (opt_pcl_textured) {

pcl_size = vpImageConvert::depthToPointCloud(I, depth_raw, depth_scale, cam_depth, pointcloud_color, &pointcloud_mutex, &mask, Z_min, Z_max);

}

else {

pcl_size = vpImageConvert::depthToPointCloud(depth_raw, depth_scale, cam_depth, pointcloud, &pointcloud_mutex, &mask, Z_min, Z_max);

}

if (opt_verbose) {

std::cout << "Segmented point cloud size: " << pcl_size << std::endl;

}

Finally, when running the point cloud viewer in the main thread, we call the vpDisplayPCL::display() method in a non-blocking mode.
if (opt_mode == DisplayMode::MONOTHREAD) {

const bool blocking_mode = false;

pcl_viewer.display(blocking_mode);

}

To run this tutorial

Learn the color of the object from which you want to extract the corresponding point cloud

$ cd $VISP_WS/visp-build/tutorial/segmentation/color
$ ./tutorial-hsv-range-tuner --hsv-thresholds calib/hsv-thresholds.yml

Visualize to corresponding point cloud
```
$ ./tutorial-hsv-segmentation-pcl-viewer --hsv-thresholds calib/hsv-thresholds.yml --texture
```
At this point, you should see something similar to the following video, where the top left image corresponds to the live stream provided by a Realsense D435 camera, the top right image to the HSV yellow color segmentation, and the bottom right image to the PCL point cloud viewer used to visualize the textured point cloud corresponding to the yellow pixels in real time.

Known issue

Segfault in PCL viewer

There is a known issue related to the call to PCLVisualizer::spinOnce() function that is used in vpDisplayPCL class. This issue is reported in https://github.com/PointCloudLibrary/pcl/issues/5237 and occurs with PCL 1.12.1 and VTK 9.1 on Ubuntu 22.04.

The workaround consists in installing the lastest PCL release.

First uninstall libpcl-dev package
```
$ sudo apt remove libpcl-dev
```

Then download and install latest PCL release from source. In our case we installed PCL 1.14.0 on Ubuntu 22.04 with:

$ mkdir -p $VISP_WS/3rdparty/pcl
$ cd $VISP_WS/3rdparty/pcl
$ git clone --branch pcl-1.14.0 https://github.com/PointCloudLibrary/pcl pcl-1.14.0
$ mkdir pcl-1.14.0/build
$ cd pcl-1.14.0/build
$ make -j$(nproc)
$ sudo make install

Finally, create a fresh ViSP configuration and build.
Warning
It could be nice to create a backup of the visp-build folder before removing it.
```
$ cd $VISP_WS
$ rm -rf visp-build
$ mkdir visp-build && cd visp-build
$ cmake ../visp
$ make -j$(nproc)
```

Known issue on MacOS

On MacOS, you can face the following error:

tutorial-display-pcl *** Terminating app due to uncaught exception 'NSInternalInconsistencyException', reason: 'NSWindow drag regions should only be invalidated on the Main Thread!'

libc++abi: terminating due to uncaught exception of type NSException

This problem seems to be due to VTK library that does not like to be run in a non-main thread on MacOS. You can use the vpDisplayPCL class in monothread mode using the method vpDisplayPCL::display. See the PCL issue for more details.

Known issue when using VTK and the server X backends

When the PCL library relies on the VTK library and X11 library as backends for the viewer, you may face the following error if you create either two viewers in threaded mode or one viewer in threaded mode and another one in the main thread:

X Error of failed request:  BadAccess (attempt to access private resource denied)
  Major opcode of failed request:  152 (GLX)
  Minor opcode of failed request:  5 (X_GLXMakeCurrent)
  Serial number of failed request:  407
  Current serial number in output stream:  407

Next tutorial

You are now ready to see how to continue with Tutorial: Image frame grabbing.