Tutorial: Threaded PCL viewer

Introduction

This tutorial shows how to use the vpDisplayPCL class.

In the next section you will find an example that shows how to use a threaded display to display two point clouds, with one suffering some noise.

The program first generate a polynomial surface, whose coordinates are expressed in the object frame. Then, a second surface is generated. It corresponds to the first surface, moved in another coordinates frame. Some noise is added to this second surface if requested by the user (by default it is not activated), to simulate sensor innacuracy. Finally, the point clouds are displayed using the vpDisplayPCL.

Requirements

To enable vpDisplayPCL class usage, and thus use this tutorial, you need to have a version of ViSP build with PCL. To see how to install PCL library, please refer to the Point Cloud Library (PCL) section.

How to run the tutorial

To see the different options of the tutorial, please run the following commands:

cd $VISP_WS/visp-build/tutorial/gui/pcl-visualizer/
$ ./tutorial-display-pcl -h

You should see a new windows that shows something similar to. It shows our two point clouds.

To run the program without adding noise to the second point cloud, you can run:

$ ./tutorial-display-pcl

To stop the program, please click in the viewer window and press the q key.

Point clouds visualization example explained

For this tutorial, we use the main program tutorial-display-pcl.cpp.

It uses the following class, which generates 3D coordinates and relies on the vpDisplayPCL to visualize data.

#ifndef _ClassUsingPclVisualizer_h_
#define _ClassUsingPclVisualizer_h_

#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_PCL_IO)
 
#include<visp3/core/vpColVector.h>
#include<visp3/gui/vpDisplayPCL.h>
 
class ClassUsingDisplayPCL
{
public:
  using PointType = pcl::PointXYZ;
private:
  VISP_NAMESPACE_ADDRESSING vpTranslationVector m_t; 
  VISP_NAMESPACE_ADDRESSING vpRotationMatrix m_R; 
  VISP_NAMESPACE_ADDRESSING vpHomogeneousMatrix m_cMo; 
 
  double m_minX; 
  double m_maxX; 
  unsigned int m_n; 
  double m_dX; // m_dX = (m_maxX - m_minX)/(m_n-1)
  double m_minY; 
  double m_maxY; 
  unsigned int m_m;  
  double m_dY; // m_dY = (m_maxY - m_minY)/(m_m-1)
 
  VISP_NAMESPACE_ADDRESSING vpDisplayPCL m_visualizer; 

  void generateControlPoints(const double &addedNoise, const unsigned int &order, pcl::PointCloud<PointType>::Ptr &base, pcl::PointCloud<PointType>::Ptr &rotated);
public:
  ClassUsingDisplayPCL(std::pair<double, double> xlimits = { -2.5,2.5 }, std::pair<double, double> ylimits = { -2.5,2.5 }, std::pair<unsigned int, unsigned int> nbPoints = { 50,50 });
 
  ~ClassUsingDisplayPCL();

  void runDemo(const double &addedNoise, const unsigned int &order, const bool &useMonothread);
};
#endif
#endif

Main code explained

Let us first have a look at the main program.

First, we include the class that uses the vpDisplayPCL object to display different point clouds:

// Tutorial include
#include "ClassUsingDisplayPCL.h"

Then, we define the default value of the program arguments.

  const double def_addedNoise = 0.; // Standard deviation of the noise added to the points.
  const unsigned int def_order = 2;  // Order of the polynomial surface used for the example.
  const std::pair<double, double> def_xlim = std::pair<double, double>(-2.5, 2.5); // Min and max X-axis coordinates.
  const std::pair<double, double> def_ylim = std::pair<double, double>(-2.5, 2.5); // Min and max Y-axis coordinates.
  const std::pair<unsigned int, unsigned int> def_reso = std::pair<unsigned int, unsigned int>(50, 50); // Number of points along the X-axis and Y-axis reciprocally.
  const DisplayMode def_mode = DisplayMode::MONOTHREAD; // Display mode that should be used.

The following program arguments are available:

      std::cout << "NAME" << std::endl;
      std::cout << "\t" << argv[0] << "  Test programm for the PCL-based point-cloud visualizer." << std::endl
        << std::endl;
      std::cout << "SYNOPSIS" << std::endl;
      std::cout << "\t" << argv[0]
        << "\t[--noise <stdev_noise>] (default: " + std::to_string(def_addedNoise) << ")\n"
        << "\t[--order <surface-order>](default: " + std::to_string(def_order) << ")\n"
        << "\t[--x-lim <xmin xmax>](default: [" + std::to_string(def_xlim.first) + ";" + std::to_string(def_xlim.second) << "])\n"
        << "\t[--y-lim <ymin ymax>](default: [" + std::to_string(def_ylim.first) + ";" + std::to_string(def_ylim.second) << "])\n"
        << "\t[--reso <x_resolution y_resolution>](default: [" + std::to_string(def_reso.first) + ";" + std::to_string(def_reso.second) << "])\n"
        << "\t[--display-mode " << getAvailableDisplayMode() << "](default: " << displayModeToString(def_mode) << ")\n"
        << "\t[--help] [-h]" << std::endl
        << std::endl;

Let us look with more details into these arguments:

• noise represents the intensity of noise along the Z-axis, expressed in the object frame, has to be added to the original surface.
• order represents the order of the polynomial surface the user wants to use running the demo.
• x-lim and y-lim represents reciproquely the X-axis and Y-axis minimum and maximum values of the polynomial surface, expressed in the object frame.
• reso represents the number of points along the X-axis and Y-axis, expressed in the object frame, are used to generate the first surface.

Then, we parse the program arguments that permit to the user to change part of the behavior of the program.

  double opt_addedNoise = def_addedNoise;
  unsigned int opt_order = def_order;
  std::pair<double, double> opt_xlim = def_xlim;
  std::pair<double, double> opt_ylim = def_ylim;
  std::pair<unsigned int, unsigned int> opt_reso = def_reso;
  DisplayMode opt_mode = def_mode;
 
  for (int i = 1; i < argc; i++) {
    if (std::string(argv[i]) == "--noise" && i + 1 < argc) {
      opt_addedNoise = atof(argv[i + 1]);
      i++;
    }
    else if (std::string(argv[i]) == "--order" && i + 1 < argc) {
      opt_order = atoi(argv[i + 1]);
      i++;
    }
    else if (std::string(argv[i]) == "--x-lim" && i + 2 < argc) {
      opt_xlim.first = atof(argv[i + 1]);
      opt_xlim.second = atof(argv[i + 2]);
      i += 2;
    }
    else if (std::string(argv[i]) == "--y-lim" && i + 2 < argc) {
      opt_ylim.first = atof(argv[i + 1]);
      opt_ylim.second = atof(argv[i + 2]);
      i += 2;
    }
    else if (std::string(argv[i]) == "--reso" && i + 2 < argc) {
      opt_reso.first = atoi(argv[i + 1]);
      opt_reso.second = atoi(argv[i + 2]);
      i += 2;
    }
    else if (std::string(argv[i]) == "--display-mode" && i + 1 < argc) {
      opt_mode = displayModeFromString(std::string(argv[i + 1]));
      i++;
    }
    else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
      std::cout << "NAME" << std::endl;
      std::cout << "\t" << argv[0] << "  Test programm for the PCL-based point-cloud visualizer." << std::endl
        << std::endl;
      std::cout << "SYNOPSIS" << std::endl;
      std::cout << "\t" << argv[0]
        << "\t[--noise <stdev_noise>] (default: " + std::to_string(def_addedNoise) << ")\n"
        << "\t[--order <surface-order>](default: " + std::to_string(def_order) << ")\n"
        << "\t[--x-lim <xmin xmax>](default: [" + std::to_string(def_xlim.first) + ";" + std::to_string(def_xlim.second) << "])\n"
        << "\t[--y-lim <ymin ymax>](default: [" + std::to_string(def_ylim.first) + ";" + std::to_string(def_ylim.second) << "])\n"
        << "\t[--reso <x_resolution y_resolution>](default: [" + std::to_string(def_reso.first) + ";" + std::to_string(def_reso.second) << "])\n"
        << "\t[--display-mode " << getAvailableDisplayMode() << "](default: " << displayModeToString(def_mode) << ")\n"
        << "\t[--help] [-h]" << std::endl
        << std::endl;
      return EXIT_SUCCESS;
    }
    else {
      std::cerr << "Option '" << argv[i] << "' is not known. Please run '" << argv[0] << " --help' to have the list of options." << std::endl;
      return EXIT_FAILURE;
    }
  }

Code of the example class explained

Generation of the polynomial surfaces used in this example

For this example, we decided to modelize a polynomial 3D surface. The Z coordinate is computed from the X and Y coordinates thanks to the following method.

double zFunction(const double &x, const double &y, const unsigned int order)
{
  const double offset(0.5);
  double z(0.);
 
  for (unsigned int n = 0; n <= order; n++) {
    for (unsigned int k = 0; k <= order - n; k++) {
      if (k + n > 0) {
        z += std::pow(x, n) * std::pow(y, k);
      }
      else {
        z += offset;
      }
    }
  }
 
  return z;
}

The constructor initializes the minimum and maximum X and Y coordinates of the polynomial surface, along with the number of points in each direction it contains. It also constructs the vpDisplayPCL object, naming the window that will open.

ClassUsingDisplayPCL::ClassUsingDisplayPCL(std::pair<double, double> xlimits, std::pair<double, double> ylimits, std::pair<unsigned int, unsigned int> nbPoints)
  : m_t(0.1, 0.1, 0.1)
  , m_R(M_PI_4, M_PI_4, M_PI_4)
  , m_cMo(m_t, m_R)
  , m_minX(xlimits.first)
  , m_maxX(xlimits.second)
  , m_n(nbPoints.first)
  , m_minY(ylimits.first)
  , m_maxY(ylimits.second)
  , m_m(nbPoints.second)
  , m_visualizer(0, 0, "Grid of points")
{
  m_dX = (m_maxX - m_minX) / (static_cast<double>(m_n) - 1.);
  m_dY = (m_maxY - m_minY) / (static_cast<double>(m_m) - 1.);
}

The following method generate two polynomial surface. If the user asked to, noise will be added to the displaced surface.

void ClassUsingDisplayPCL::generateControlPoints(const double &addedNoise, const unsigned int &order, pcl::PointCloud<PointType>::Ptr &base, pcl::PointCloud<PointType>::Ptr &rotated)
{
  // Create control points
  bool initialize_base = (base ? false : true);
  if (initialize_base) {
    base = std::make_shared<pcl::PointCloud<PointType>>(m_n, m_m);
  }
  bool initialize_rotated = (rotated ? false : true);
  if (initialize_rotated) {
    rotated = std::make_shared<pcl::PointCloud<PointType>>(m_n, m_m);
  }
 
  // Noise generator for the observed points
  vpGaussRand r;
  r.setSigmaMean(addedNoise, 0.);
  r.seed(vpTime::measureTimeMicros());
 
  for (unsigned int j = 0; j < m_m; j++) {
    for (unsigned int i = 0; i < m_n; i++) {
      // Creating model, expressed in the object frame
      double oX = m_minX + static_cast<double>(i) * m_dX;
      double oY = m_minY + static_cast<double>(j) * m_dY;
      double oZ = zFunction(oX, oY, order);
 
      // Setting the point coordinates of the first point cloud in
      // the object frame
      std::vector<double> point = { oX, oY, oZ,1. };
      vpColVector oCoords = vpColVector(point);
      if (initialize_base) {
        (*base)(i, j).x = oCoords[0];
        (*base)(i, j).y = oCoords[1];
        (*base)(i, j).z = oCoords[2];
      }
 
      // Moving the point into another coordinate frame
      vpColVector cCoords = m_cMo * oCoords;
      (*rotated)(i, j).x = cCoords[0];
      (*rotated)(i, j).y = cCoords[1];
 
      // Potentially adding some noise if the user asked to
      double noise = r();
      (*rotated)(i, j).z = cCoords[2] + noise;
    }
  }
}

How to use the vpDisplayPCL class to display the point clouds

To use the vpDisplayPCL class, you must first add the surfaces you want to display. For a textureless point cloud, you can either choose the color to use to render it or you can use the default color. Please refer to vpDisplayPCL::addPointCloud for more information.

  // Adding a point cloud for which we don't chose the color
  std::mutex mutex_base;
  vpColorBlindFriendlyPalette color_base(vpColorBlindFriendlyPalette::Palette::Yellow);
  m_visualizer.addPointCloud(mutex_base, base, "Base", color_base.to_vpColor());
 
  // Adding a point cloud for which we chose the color
  std::mutex mutex_rotated;
  vpColorBlindFriendlyPalette color(vpColorBlindFriendlyPalette::Palette::Purple);
  m_visualizer.addPointCloud(mutex_rotated, rotated, "RotatedWithNoise", color.to_vpColor());

To update the surfaces over time, please use the following lines of codes:

    {
      std::lock_guard lg_base(mutex_base);
      std::lock_guard lg_rotated(mutex_rotated);
      generateControlPoints(addedNoise, order, base, rotated);
    }

As you can see, you don't have to do anything to the vpDisplayPCL class, but you have to lock the mutex in order to avoid concurrency access.

Finally, if you decided to run the visualizer in monothread mode, you have to call the vpDIsplayPCL::display() method to refresh the visualizer:

    if (useMonothread) {
      const bool blocking_mode = false;
      m_visualizer.display(blocking_mode);
    }

Known issues

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 on Ubuntu 22.04

On Ubuntu 22.04, you can face the following error:

Thread 2 "tutorial-pcl-vi" received signal SIGSEGV, Segmentation fault.
0x00007ffff7304b10 in _XEventsQueued () from /lib/x86_64-linux-gnu/libX11.so.6
0x00007ffff7304b10 in _XEventsQueued () at /lib/x86_64-linux-gnu/libX11.so.6
0x00007ffff72f11a1 in XPending () at /lib/x86_64-linux-gnu/libX11.so.6
0x00007fffecf65b8f in vtkXRenderWindowInteractor::StartEventLoop() () at /lib/x86_64-linux-gnu/libvtkRenderingUI-9.1.so.1
0x00007ffff6ee3f8c in pcl::visualization::PCLVisualizer::spinOnce(int, bool) () at /lib/x86_64-linux-gnu/libpcl_visualization.so.1.12
0x00007ffff7fa5c49 in vpPclVisualizer::loopThread() (this=0x7fffffffd720) at /usr/include/c++/11/bits/shared_ptr_base.h:1295

This is a known compatibility issue between PCL library and VTK library.

The vpDisplayPCL can be used in monothread mode, or you may try to install PCL from source and then recompile ViSP.

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