vpDisplayPCL.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Display a point cloud using PCL library.
 */
 
#ifndef VP_DISPLAY_PCL_H
#define VP_DISPLAY_PCL_H
 
#include <visp3/core/vpConfig.h>
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS)
 
#include <mutex>
#include <thread>
#include <string>
 
#include <visp3/core/vpColor.h>
 
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpDisplayPCL
{
public:
  vpDisplayPCL(int posx = 0, int posy = 0, const std::string &window_name = "");
  vpDisplayPCL(unsigned int width, unsigned int height, int posx = 0, int posy = 0, const std::string &window_name = "");
  ~vpDisplayPCL();
 
  void setVerbose(bool verbose);
  void display(const bool &blocking = false);
  void startThread(const bool &colorThread = false);
  void startThread(std::mutex &mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud, const std::string &name = "", const vpColor &color = vpColor::red);
  void startThread(std::mutex &mutex, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud);
  void addPointCloud(std::mutex &mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud, const std::string &name = "", const vpColor &color = vpColor::red);
  void addPointCloud(std::mutex &mutex, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud);
  void setPosition(int posx, int posy);
  void setWindowName(const std::string &window_name);
  void stop();
private:
  void run();
  void runColor();
  void createViewer();
  void insertLegend(const size_t &id);
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
  typedef struct PointCloudHandling
  {
    bool m_do_init; 
    std::string m_name; 
    static unsigned int s_nb; 

    std::string createName()
    {
      std::stringstream ss;
      ss << "Point cloud " << s_nb;
      return ss.str();
    }

    void init(const std::string &name)
    {
      if (name.empty()) {
        m_name = createName();
      }
      else {
        m_name = name;
      }
      m_do_init = true;
      ++s_nb;
    }

    void init()
    {
      init(createName());
    }

    PointCloudHandling()
    {
      init();
    }

    PointCloudHandling(const std::string &name)
    {
      init(name);
    }
  } PointCloudHandling;
 
  typedef struct XYZPointCloudHandling : public PointCloudHandling
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_pcl; 
    vpColor m_color; 

    XYZPointCloudHandling(pcl::PointCloud<pcl::PointXYZ>::Ptr pcl, const vpColor &color)
      : PointCloudHandling()
      , m_pcl(pcl)
      , m_color(color)
    { }

    XYZPointCloudHandling(pcl::PointCloud<pcl::PointXYZ>::Ptr pcl, const std::string &name, const vpColor &color)
      : PointCloudHandling(name)
      , m_pcl(pcl)
      , m_color(color)
    { }
  } XYZPointCloudHandling;

  typedef struct ColoredPointCloudHandling : public PointCloudHandling
  {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr m_pcl; 

    ColoredPointCloudHandling(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl)
      : PointCloudHandling()
      , m_pcl(pcl)
    { }

    ColoredPointCloudHandling(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl, const std::string &name)
      : PointCloudHandling(name)
      , m_pcl(pcl)
    { }
  } ColoredPointCloudHandling;
#endif
 
  bool m_stop;
  bool m_thread_running; 
  bool m_verbose;
  std::thread m_thread; 
  std::mutex m_mutex_vector; 
  std::vector<std::pair<std::mutex &, XYZPointCloudHandling>> mv_xyz_pcl; 
  std::vector<pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>> mv_xyz_handlers; 
  std::vector<std::pair<std::mutex &, ColoredPointCloudHandling>> mv_colored_pcl; 
  std::vector<pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>> mv_color_handlers; 
  unsigned int m_width;
  unsigned int m_height;
  int m_posx;
  int m_posy;
  std::string m_window_name;
  pcl::visualization::PCLVisualizer::Ptr m_viewer;
};
END_VISP_NAMESPACE
#endif
 
#endif