vpDisplayOpenCV.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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:
 * Image display.
 *
*****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#if defined(HAVE_OPENCV_HIGHGUI)
 
#include <cmath> // std::fabs
#include <iostream>
#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>
 
// Display stuff
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayOpenCV.h>
 
// debug / exception
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplayException.h>
 
#include <opencv2/core/core_c.h> // for CV_FILLED versus cv::FILLED
 
#if defined(HAVE_OPENCV_IMGPROC)
#include <opencv2/imgproc/imgproc.hpp>
#endif
 
#ifndef CV_RGB
#define CV_RGB(r, g, b) cv::Scalar((b), (g), (r), 0)
#endif
 
#ifdef VISP_HAVE_X11
#include <visp3/gui/vpDisplayX.h> // to get screen resolution
#elif defined(_WIN32)
#include <windows.h>
#endif
 
std::vector<std::string> vpDisplayOpenCV::m_listTitles = std::vector<std::string>();
unsigned int vpDisplayOpenCV::m_nbWindows = 0;
 
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I, vpScaleType scaleType)
  : vpDisplay(),
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{
  setScale(scaleType, I.getWidth(), I.getHeight());
  init(I);
}
 
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I, int x, int y, const std::string &title,
                                 vpScaleType scaleType)
  : vpDisplay(),
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{
  setScale(scaleType, I.getWidth(), I.getHeight());
  init(I, x, y, title);
}
 
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I, vpScaleType scaleType)
  :
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{
  setScale(scaleType, I.getWidth(), I.getHeight());
  init(I);
}
 
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I, int x, int y, const std::string &title, vpScaleType scaleType)
  :
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{
  setScale(scaleType, I.getWidth(), I.getHeight());
  init(I, x, y, title);
}
 
vpDisplayOpenCV::vpDisplayOpenCV(int x, int y, const std::string &title)
  :
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{
  m_windowXPosition = x;
  m_windowYPosition = y;
 
  if (!title.empty()) {
    m_title = title;
  }
  else {
    std::ostringstream s;
    s << m_nbWindows++;
    m_title = std::string("Window ") + s.str();
  }
 
  bool isInList;
  do {
    isInList = false;
    for (size_t i = 0; i < m_listTitles.size(); i++) {
      if (m_listTitles[i] == m_title) {
        std::ostringstream s;
        s << m_nbWindows++;
        m_title = std::string("Window ") + s.str();
        isInList = true;
        break;
      }
    }
  } while (isInList);
 
  m_listTitles.push_back(m_title);
}
 
vpDisplayOpenCV::vpDisplayOpenCV()
  :
  m_background(), col(nullptr), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
  fontHeight(10), x_move(0), y_move(0), move(false), x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
  x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false), x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
  x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false), x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false), x_rbuttonup(0),
  y_rbuttonup(0), rbuttonup(false)
{ }
 
vpDisplayOpenCV::~vpDisplayOpenCV()
{
  closeDisplay();
}
 
void vpDisplayOpenCV::init(vpImage<unsigned char> &I, int x, int y, const std::string &title)
{
  if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
  }
  setScale(m_scaleType, I.getWidth(), I.getHeight());
  init(I.getWidth(), I.getHeight(), x, y, title);
  I.display = this;
  m_displayHasBeenInitialized = true;
}
 
void vpDisplayOpenCV::init(vpImage<vpRGBa> &I, int x, int y, const std::string &title)
{
  if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
  }
 
  setScale(m_scaleType, I.getWidth(), I.getHeight());
  init(I.getWidth(), I.getHeight(), x, y, title);
  I.display = this;
  m_displayHasBeenInitialized = true;
}
 
void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, const std::string &title)
{
  setScale(m_scaleType, w, h);
 
  this->m_width = w / m_scale;
  this->m_height = h / m_scale;
 
  if (x != -1) {
    this->m_windowXPosition = x;
  }
  if (y != -1) {
    this->m_windowYPosition = y;
  }
  int flags = cv::WINDOW_AUTOSIZE;
 
  if (m_title.empty()) {
    if (!title.empty()) {
      m_title = std::string(title);
    }
    else {
 
      std::ostringstream s;
      s << m_nbWindows++;
      m_title = std::string("Window ") + s.str();
    }
 
    bool isInList;
    do {
      isInList = false;
      for (size_t i = 0; i < m_listTitles.size(); i++) {
        if (m_listTitles[i] == m_title) {
          std::ostringstream s;
          s << m_nbWindows++;
          m_title = std::string("Window ") + s.str();
          isInList = true;
          break;
        }
      }
    } while (isInList);
 
    m_listTitles.push_back(m_title);
  }
 
  /* Create the window*/
  cv::namedWindow(this->m_title, flags);
  cv::moveWindow(this->m_title.c_str(), this->m_windowXPosition, this->m_windowYPosition);
 
  move = false;
  lbuttondown = false;
  mbuttondown = false;
  rbuttondown = false;
  lbuttonup = false;
  mbuttonup = false;
  rbuttonup = false;
 
  cv::setMouseCallback(this->m_title, on_mouse, this);
  col = new cv::Scalar[vpColor::id_unknown];
 
  /* Create color */
  vpColor pcolor; // Predefined colors
  pcolor = vpColor::lightBlue;
  col[vpColor::id_lightBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::blue;
  col[vpColor::id_blue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::darkBlue;
  col[vpColor::id_darkBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::lightRed;
  col[vpColor::id_lightRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::red;
  col[vpColor::id_red] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::darkRed;
  col[vpColor::id_darkRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::lightGreen;
  col[vpColor::id_lightGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::green;
  col[vpColor::id_green] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::darkGreen;
  col[vpColor::id_darkGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::yellow;
  col[vpColor::id_yellow] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::cyan;
  col[vpColor::id_cyan] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::orange;
  col[vpColor::id_orange] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::purple;
  col[vpColor::id_purple] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::white;
  col[vpColor::id_white] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::black;
  col[vpColor::id_black] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::lightGray;
  col[vpColor::id_lightGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::gray;
  col[vpColor::id_gray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
  pcolor = vpColor::darkGray;
  col[vpColor::id_darkGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
 
  int thickness = 1;
  cv::Size fontSize;
  int baseline;
  fontSize = cv::getTextSize("A", font, fontScale, thickness, &baseline);
 
  fontHeight = fontSize.height + baseline;
  m_displayHasBeenInitialized = true;
}
 
void vpDisplayOpenCV::setFont(const std::string & /* font */) { vpERROR_TRACE("Not yet implemented"); }
 
void vpDisplayOpenCV::setTitle(const std::string & /* title */)
{
  //  static bool warn_displayed = false;
  //  if (! warn_displayed) {
  //    vpTRACE("Not implemented");
  //    warn_displayed = true;
  //  }
}
 
void vpDisplayOpenCV::setWindowPosition(int winx, int winy)
{
  if (m_displayHasBeenInitialized) {
    this->m_windowXPosition = winx;
    this->m_windowYPosition = winy;
    cv::moveWindow(this->m_title.c_str(), winx, winy);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &I)
{
  if (m_displayHasBeenInitialized) {
    int depth = CV_8U;
    int channels = 3;
    cv::Size size((int)m_width, (int)m_height);
    if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height ||
        m_background.cols != (int)m_width) {
      m_background = cv::Mat(size, CV_MAKETYPE(depth, channels));
    }
 
    if (m_scale == 1) {
      for (unsigned int i = 0; i < m_height; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width);
        for (unsigned int j = 0; j < m_width; j++) {
          unsigned char val = I[i][j];
          *(dst_24++) = val;
          *(dst_24++) = val;
          *(dst_24++) = val;
        }
      }
    }
    else {
      for (unsigned int i = 0; i < m_height; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width);
        for (unsigned int j = 0; j < m_width; j++) {
          unsigned char val = I[i * m_scale][j * m_scale];
          *(dst_24++) = val;
          *(dst_24++) = val;
          *(dst_24++) = val;
        }
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, unsigned int w,
                                      unsigned int h)
{
  if (m_displayHasBeenInitialized) {
    int depth = CV_8U;
    int channels = 3;
    cv::Size size((int)m_width, (int)m_height);
    if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height ||
        m_background.cols != (int)m_width) {
      m_background = cv::Mat(size, CV_MAKETYPE(depth, channels));
    }
 
    if (m_scale == 1) {
      unsigned int i_min = (unsigned int)iP.get_i();
      unsigned int j_min = (unsigned int)iP.get_j();
      unsigned int i_max = std::min<unsigned int>(i_min + h, m_height);
      unsigned int j_max = std::min<unsigned int>(j_min + w, m_width);
      for (unsigned int i = i_min; i < i_max; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3);
        for (unsigned int j = j_min; j < j_max; j++) {
          unsigned char val = I[i][j];
          *(dst_24++) = val;
          *(dst_24++) = val;
          *(dst_24++) = val;
        }
      }
    }
    else {
      int i_min = std::max<int>((int)ceil(iP.get_i() / m_scale), 0);
      int j_min = std::max<int>((int)ceil(iP.get_j() / m_scale), 0);
      int i_max = std::min<int>((int)ceil((iP.get_i() + h) / m_scale), (int)m_height);
      int j_max = std::min<int>((int)ceil((iP.get_j() + w) / m_scale), (int)m_width);
      for (int i = i_min; i < i_max; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3);
        for (int j = j_min; j < j_max; j++) {
          unsigned char val = I[i * m_scale][j * m_scale];
          *(dst_24++) = val;
          *(dst_24++) = val;
          *(dst_24++) = val;
        }
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &I)
{
 
  if (m_displayHasBeenInitialized) {
    int depth = CV_8U;
    int channels = 3;
    cv::Size size((int)this->m_width, (int)this->m_height);
    if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height ||
        m_background.cols != (int)m_width) {
      m_background = cv::Mat(size, CV_MAKETYPE(depth, channels));
    }
 
    if (m_scale == 1) {
      for (unsigned int i = 0; i < m_height; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width);
        for (unsigned int j = 0; j < m_width; j++) {
          vpRGBa val = I[i][j];
          *(dst_24++) = val.B;
          *(dst_24++) = val.G;
          *(dst_24++) = val.R;
        }
      }
    }
    else {
      for (unsigned int i = 0; i < m_height; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width);
        for (unsigned int j = 0; j < m_width; j++) {
          vpRGBa val = I[i * m_scale][j * m_scale];
          *(dst_24++) = val.B;
          *(dst_24++) = val.G;
          *(dst_24++) = val.R;
        }
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, unsigned int w, unsigned int h)
{
  if (m_displayHasBeenInitialized) {
    int depth = CV_8U;
    int channels = 3;
    cv::Size size((int)this->m_width, (int)this->m_height);
    if (m_background.channels() != channels || m_background.depth() != depth || m_background.rows != (int)m_height ||
        m_background.cols != (int)m_width) {
      m_background = cv::Mat(size, CV_MAKETYPE(depth, channels));
    }
 
    if (m_scale == 1) {
      unsigned int i_min = (unsigned int)iP.get_i();
      unsigned int j_min = (unsigned int)iP.get_j();
      unsigned int i_max = std::min<unsigned int>(i_min + h, m_height);
      unsigned int j_max = std::min<unsigned int>(j_min + w, m_width);
      for (unsigned int i = i_min; i < i_max; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3);
        for (unsigned int j = j_min; j < j_max; j++) {
          vpRGBa val = I[i][j];
          *(dst_24++) = val.B;
          *(dst_24++) = val.G;
          *(dst_24++) = val.R;
        }
      }
    }
    else {
      int i_min = std::max<int>((int)ceil(iP.get_i() / m_scale), 0);
      int j_min = std::max<int>((int)ceil(iP.get_j() / m_scale), 0);
      int i_max = std::min<int>((int)ceil((iP.get_i() + h) / m_scale), (int)m_height);
      int j_max = std::min<int>((int)ceil((iP.get_j() + w) / m_scale), (int)m_width);
      for (int i = i_min; i < i_max; i++) {
        unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3);
        for (int j = j_min; j < j_max; j++) {
          vpRGBa val = I[i * m_scale][j * m_scale];
          *(dst_24++) = val.B;
          *(dst_24++) = val.G;
          *(dst_24++) = val.R;
        }
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); }
 
void vpDisplayOpenCV::closeDisplay()
{
  if (col != nullptr) {
    delete[] col;
    col = nullptr;
  }
  if (m_displayHasBeenInitialized) {
    cv::destroyWindow(this->m_title);
    for (size_t i = 0; i < m_listTitles.size(); i++) {
      if (m_title == m_listTitles[i]) {
        m_listTitles.erase(m_listTitles.begin() + (long int)i);
        break;
      }
    }
 
    m_title.clear();
 
    m_displayHasBeenInitialized = false;
  }
}
 
void vpDisplayOpenCV::flushDisplay()
{
  if (m_displayHasBeenInitialized) {
    cv::imshow(this->m_title, m_background);
    cv::waitKey(5);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned int /*width*/,
                                      const unsigned int /*height*/)
{
  if (m_displayHasBeenInitialized) {
    cv::imshow(this->m_title.c_str(), m_background);
    cv::waitKey(5);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::clearDisplay(const vpColor & /* color */)
{
  static bool warn_displayed = false;
  if (!warn_displayed) {
    vpTRACE("Not implemented");
    warn_displayed = true;
  }
}
 
void vpDisplayOpenCV::displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
                                   unsigned int w, unsigned int h, unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    double a = ip2.get_i() - ip1.get_i();
    double b = ip2.get_j() - ip1.get_j();
    double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
 
    // if ((a==0)&&(b==0))
    if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
        (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
      // DisplayCrossLarge(i1,j1,3,col) ;
    }
    else {
      a /= lg;
      b /= lg;
 
      vpImagePoint ip3;
      ip3.set_i(ip2.get_i() - w * a);
      ip3.set_j(ip2.get_j() - w * b);
 
      vpImagePoint ip4;
      ip4.set_i(ip3.get_i() - b * h);
      ip4.set_j(ip3.get_j() + a * h);
 
      if (lg > 2 * vpImagePoint::distance(ip2, ip4))
        displayLine(ip2, ip4, color, thickness);
 
      ip4.set_i(ip3.get_i() + b * h);
      ip4.set_j(ip3.get_j() - a * h);
 
      if (lg > 2 * vpImagePoint::distance(ip2, ip4))
        displayLine(ip2, ip4, color, thickness);
 
      displayLine(ip1, ip2, color, thickness);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayText(const vpImagePoint &ip, const std::string &text, const vpColor &color)
{
  if (m_displayHasBeenInitialized) {
    if (color.id < vpColor::id_unknown) {
      cv::putText(m_background, text,
                  cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)),
                  font, fontScale, col[color.id]);
    }
    else {
      cvcolor = CV_RGB(color.R, color.G, color.B);
      cv::putText(m_background, text,
                  cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale + fontHeight)),
                  font, fontScale, cvcolor);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
void vpDisplayOpenCV::displayCircle(const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill,
                                    unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    int x = vpMath::round(center.get_u() / m_scale);
    int y = vpMath::round(center.get_v() / m_scale);
    int r = static_cast<int>(radius / m_scale);
    cv::Scalar cv_color;
    if (color.id < vpColor::id_unknown) {
      cv_color = col[color.id];
    }
    else {
      cv_color = CV_RGB(color.R, color.G, color.B);
    }
 
    if (fill == false) {
      int cv_thickness = static_cast<int>(thickness);
      cv::circle(m_background, cv::Point(x, y), r, cv_color, cv_thickness);
    }
    else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
      int filled = cv::FILLED;
#else
      int filled = CV_FILLED;
#endif
      double opacity = static_cast<double>(color.A) / 255.0;
      overlay([x, y, r, cv_color, filled](cv::Mat image) { cv::circle(image, cv::Point(x, y), r, cv_color, filled); },
              opacity);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color,
                                   unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    vpImagePoint top, bottom, left, right;
    top.set_i(ip.get_i() - size / 2);
    top.set_j(ip.get_j());
    bottom.set_i(ip.get_i() + size / 2);
    bottom.set_j(ip.get_j());
    left.set_i(ip.get_i());
    left.set_j(ip.get_j() - size / 2);
    right.set_i(ip.get_i());
    right.set_j(ip.get_j() + size / 2);
    displayLine(top, bottom, color, thickness);
    displayLine(left, right, color, thickness);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
                                     unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    vpImagePoint ip1_ = ip1;
    vpImagePoint ip2_ = ip2;
 
    double size = 10. * m_scale;
    double length = sqrt(vpMath::sqr(ip2_.get_i() - ip1_.get_i()) + vpMath::sqr(ip2_.get_j() - ip1_.get_j()));
    bool vertical_line = (int)ip2_.get_j() == (int)ip1_.get_j();
    if (vertical_line) {
      if (ip2_.get_i() < ip1_.get_i()) {
        std::swap(ip1_, ip2_);
      }
    }
    else if (ip2_.get_j() < ip1_.get_j()) {
      std::swap(ip1_, ip2_);
    }
 
    double diff_j = vertical_line ? 1 : ip2_.get_j() - ip1_.get_j();
    double deltaj = size / length * diff_j;
    double deltai = size / length * (ip2_.get_i() - ip1_.get_i());
    double slope = (ip2_.get_i() - ip1_.get_i()) / diff_j;
    double orig = ip1_.get_i() - slope * ip1_.get_j();
 
    if (vertical_line) {
      for (unsigned int i = (unsigned int)ip1_.get_i(); i < ip2_.get_i(); i += (unsigned int)(2 * deltai)) {
        double j = ip1_.get_j();
        displayLine(vpImagePoint(i, j), vpImagePoint(i + deltai, j), color, thickness);
      }
    }
    else {
      for (unsigned int j = (unsigned int)ip1_.get_j(); j < ip2_.get_j(); j += (unsigned int)(2 * deltaj)) {
        double i = slope * j + orig;
        displayLine(vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
                                  unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    if (color.id < vpColor::id_unknown) {
      cv::line(m_background, cv::Point(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)),
               cv::Point(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), col[color.id],
               (int)thickness);
    }
    else {
      cvcolor = CV_RGB(color.R, color.G, color.B);
      cv::line(m_background, cv::Point(vpMath::round(ip1.get_u() / m_scale), vpMath::round(ip1.get_v() / m_scale)),
               cv::Point(vpMath::round(ip2.get_u() / m_scale), vpMath::round(ip2.get_v() / m_scale)), cvcolor,
               (int)thickness);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    for (unsigned int i = 0; i < thickness; i++) {
      if (color.id < vpColor::id_unknown) {
        cv::line(m_background, cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)),
                 cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)),
                 col[color.id], (int)thickness);
      }
      else {
        cvcolor = CV_RGB(color.R, color.G, color.B);
        cv::line(m_background, cv::Point(vpMath::round(ip.get_u() / m_scale), vpMath::round(ip.get_v() / m_scale)),
                 cv::Point(vpMath::round(ip.get_u() / m_scale + thickness - 1), vpMath::round(ip.get_v() / m_scale)),
                 cvcolor, (int)thickness);
      }
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, unsigned int w, unsigned int h,
                                       const vpColor &color, bool fill, unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    int left = vpMath::round(topLeft.get_u() / m_scale);
    int top = vpMath::round(topLeft.get_v() / m_scale);
    int right = vpMath::round((topLeft.get_u() + w) / m_scale);
    int bottom = vpMath::round((topLeft.get_v() + h) / m_scale);
    cv::Scalar cv_color;
    if (color.id < vpColor::id_unknown) {
      cv_color = col[color.id];
    }
    else {
      cv_color = CV_RGB(color.R, color.G, color.B);
    }
 
    if (fill == false) {
      int cv_thickness = static_cast<int>(thickness);
      cv::rectangle(m_background, cv::Point(left, top), cv::Point(right, bottom), cv_color, cv_thickness);
    }
    else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
      int filled = cv::FILLED;
#else
      int filled = CV_FILLED;
#endif
      double opacity = static_cast<double>(color.A) / 255.0;
      overlay([left, top, right, bottom, cv_color, filled](cv::Mat image) {
        cv::rectangle(image, cv::Point(left, top), cv::Point(right, bottom), cv_color, filled);
          },
          opacity);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
void vpDisplayOpenCV::displayRectangle(const vpImagePoint &topLeft, const vpImagePoint &bottomRight,
                                       const vpColor &color, bool fill, unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    int left = vpMath::round(topLeft.get_u() / m_scale);
    int top = vpMath::round(topLeft.get_v() / m_scale);
    int right = vpMath::round(bottomRight.get_u() / m_scale);
    int bottom = vpMath::round(bottomRight.get_v() / m_scale);
    cv::Scalar cv_color;
    if (color.id < vpColor::id_unknown) {
      cv_color = col[color.id];
    }
    else {
      cv_color = CV_RGB(color.R, color.G, color.B);
    }
 
    if (fill == false) {
      int cv_thickness = static_cast<int>(thickness);
      cv::rectangle(m_background, cv::Point(left, top), cv::Point(right, bottom), cv_color, cv_thickness);
    }
    else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
      int filled = cv::FILLED;
#else
      int filled = CV_FILLED;
#endif
      double opacity = static_cast<double>(color.A) / 255.0;
      overlay([left, top, right, bottom, cv_color, filled](cv::Mat image) {
        cv::rectangle(image, cv::Point(left, top), cv::Point(right, bottom), cv_color, filled);
          },
          opacity);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill, unsigned int thickness)
{
  if (m_displayHasBeenInitialized) {
    int left = vpMath::round(rectangle.getLeft() / m_scale);
    int top = vpMath::round(rectangle.getTop() / m_scale);
    int right = vpMath::round(rectangle.getRight() / m_scale);
    int bottom = vpMath::round(rectangle.getBottom() / m_scale);
    cv::Scalar cv_color;
    if (color.id < vpColor::id_unknown) {
      cv_color = col[color.id];
    }
    else {
      cv_color = CV_RGB(color.R, color.G, color.B);
    }
 
    if (fill == false) {
      int cv_thickness = static_cast<int>(thickness);
      cv::rectangle(m_background, cv::Point(left, top), cv::Point(right, bottom), cv_color, cv_thickness);
    }
    else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
      int filled = cv::FILLED;
#else
      int filled = CV_FILLED;
#endif
      double opacity = static_cast<double>(color.A) / 255.0;
      overlay([left, top, right, bottom, cv_color, filled](cv::Mat image) {
        cv::rectangle(image, cv::Point(left, top), cv::Point(right, bottom), cv_color, filled);
          },
          opacity);
    }
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
bool vpDisplayOpenCV::getClick(bool blocking)
{
  bool ret = false;
  if (m_displayHasBeenInitialized) {
    flushDisplay();
    if (blocking) {
      lbuttondown = false;
      mbuttondown = false;
      rbuttondown = false;
    }
    do {
      if (lbuttondown) {
        ret = true;
        lbuttondown = false;
      }
      if (mbuttondown) {
        ret = true;
        mbuttondown = false;
      }
      if (rbuttondown) {
        ret = true;
        rbuttondown = false;
      }
      if (blocking) {
        cv::waitKey(10);
      }
    } while (ret == false && blocking == true);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  return ret;
}
 
bool vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
{
  bool ret = false;
 
  if (m_displayHasBeenInitialized) {
    flushDisplay();
 
    double u, v;
 
    if (blocking) {
      lbuttondown = false;
      mbuttondown = false;
      rbuttondown = false;
    }
    do {
      if (lbuttondown) {
        ret = true;
        u = (unsigned int)x_lbuttondown * m_scale;
        v = (unsigned int)y_lbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        lbuttondown = false;
      }
      if (mbuttondown) {
        ret = true;
        u = (unsigned int)x_mbuttondown * m_scale;
        v = (unsigned int)y_mbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        mbuttondown = false;
      }
      if (rbuttondown) {
        ret = true;
        u = (unsigned int)x_rbuttondown * m_scale;
        v = (unsigned int)y_rbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        rbuttondown = false;
      }
      if (blocking) {
        cv::waitKey(10);
      }
    } while (ret == false && blocking == true);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  return ret;
}
 
bool vpDisplayOpenCV::getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
  bool ret = false;
 
  if (m_displayHasBeenInitialized) {
    // flushDisplay() ;
    double u, v;
    if (blocking) {
      lbuttondown = false;
      mbuttondown = false;
      rbuttondown = false;
    }
    do {
      if (lbuttondown) {
        ret = true;
        u = (unsigned int)x_lbuttondown * m_scale;
        v = (unsigned int)y_lbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button1;
        lbuttondown = false;
      }
      if (mbuttondown) {
        ret = true;
        u = (unsigned int)x_mbuttondown * m_scale;
        v = (unsigned int)y_mbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button2;
        mbuttondown = false;
      }
      if (rbuttondown) {
        ret = true;
        u = (unsigned int)x_rbuttondown * m_scale;
        v = (unsigned int)y_rbuttondown * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button3;
        rbuttondown = false;
      }
      if (blocking) {
        cv::waitKey(10);
      }
    } while (ret == false && blocking == true);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  return ret;
}
 
bool vpDisplayOpenCV::getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
  bool ret = false;
  if (m_displayHasBeenInitialized) {
    // flushDisplay() ;
    double u, v;
    if (blocking) {
      lbuttonup = false;
      mbuttonup = false;
      rbuttonup = false;
    }
    do {
      if (lbuttonup) {
        ret = true;
        u = (unsigned int)x_lbuttonup * m_scale;
        v = (unsigned int)y_lbuttonup * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button1;
        lbuttonup = false;
      }
      if (mbuttonup) {
        ret = true;
        u = (unsigned int)x_mbuttonup * m_scale;
        v = (unsigned int)y_mbuttonup * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button2;
        mbuttonup = false;
      }
      if (rbuttonup) {
        ret = true;
        u = (unsigned int)x_rbuttonup * m_scale;
        v = (unsigned int)y_rbuttonup * m_scale;
        ip.set_u(u);
        ip.set_v(v);
        button = vpMouseButton::button3;
        rbuttonup = false;
      }
      if (blocking) {
        cv::waitKey(10);
      }
    } while (ret == false && blocking == true);
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  return ret;
}
 
/*
  Gets the displayed image (including the overlay plane)
  and returns an RGBa image.
*/
void vpDisplayOpenCV::getImage(vpImage<vpRGBa> &I)
{
  vpImageConvert::convert(m_background, I);
  // should certainly be optimized.
}
 
void vpDisplayOpenCV::on_mouse(int event, int x, int y, int /*flags*/, void *display)
{
  vpDisplayOpenCV *disp = static_cast<vpDisplayOpenCV *>(display);
  switch (event) {
  case cv::EVENT_MOUSEMOVE:
  {
    disp->move = true;
    disp->x_move = x;
    disp->y_move = y;
    break;
  }
  case cv::EVENT_LBUTTONDOWN:
  {
    disp->lbuttondown = true;
    disp->x_lbuttondown = x;
    disp->y_lbuttondown = y;
    break;
  }
  case cv::EVENT_MBUTTONDOWN:
  {
    disp->mbuttondown = true;
    disp->x_mbuttondown = x;
    disp->y_mbuttondown = y;
    break;
  }
  case cv::EVENT_RBUTTONDOWN:
  {
    disp->rbuttondown = true;
    disp->x_rbuttondown = x;
    disp->y_rbuttondown = y;
    break;
  }
  case cv::EVENT_LBUTTONUP:
  {
    disp->lbuttonup = true;
    disp->x_lbuttonup = x;
    disp->y_lbuttonup = y;
    break;
  }
  case cv::EVENT_MBUTTONUP:
  {
    disp->mbuttonup = true;
    disp->x_mbuttonup = x;
    disp->y_mbuttonup = y;
    break;
  }
  case cv::EVENT_RBUTTONUP:
  {
    disp->rbuttonup = true;
    disp->x_rbuttonup = x;
    disp->y_rbuttonup = y;
    break;
  }
 
  default:
    break;
  }
}
 
bool vpDisplayOpenCV::getKeyboardEvent(bool blocking)
{
  if (m_displayHasBeenInitialized) {
    int delay;
    flushDisplay();
    if (blocking)
      delay = 0;
    else
      delay = 10;
 
    int key_pressed = cv::waitKey(delay);
 
    if (key_pressed == -1)
      return false;
    return true;
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  // return false; // Never reached after throw()
}
bool vpDisplayOpenCV::getKeyboardEvent(std::string &key, bool blocking)
{
  if (m_displayHasBeenInitialized) {
    int delay;
    flushDisplay();
    if (blocking)
      delay = 0;
    else
      delay = 10;
 
    int key_pressed = cv::waitKey(delay);
    if (key_pressed == -1)
      return false;
    else {
      // std::cout << "Key pressed: \"" << key_pressed << "\"" << std::endl;
      std::stringstream ss;
      ss << key_pressed;
      key = ss.str();
    }
    return true;
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  // return false; // Never reached after throw()
}
 
bool vpDisplayOpenCV::getPointerMotionEvent(vpImagePoint &ip)
{
  bool ret = false;
 
  if (m_displayHasBeenInitialized) {
    // flushDisplay() ;
    if (move) {
      ret = true;
      double u = (unsigned int)x_move / m_scale;
      double v = (unsigned int)y_move / m_scale;
      ip.set_u(u);
      ip.set_v(v);
      move = false;
    }
  }
 
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
  return ret;
}
 
bool vpDisplayOpenCV::getPointerPosition(vpImagePoint &ip)
{
  if (m_displayHasBeenInitialized) {
    bool moved = getPointerMotionEvent(ip);
    if (!moved) {
      double u, v;
      u = (unsigned int)x_move / m_scale;
      v = (unsigned int)y_move / m_scale;
      ip.set_u(u);
      ip.set_v(v);
    }
    return false;
  }
  else {
    throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
  }
}
 
void vpDisplayOpenCV::getScreenSize(unsigned int &w, unsigned int &h)
{
  w = h = 0;
 
#if defined(VISP_HAVE_X11)
  vpDisplayX d;
  d.getScreenSize(w, h);
#elif defined(VISP_HAVE_XRANDR)
  std::string command = "xrandr | grep '*'";
  FILE *fpipe = (FILE *)popen(command.c_str(), "r");
  char line[256];
  while (fgets(line, sizeof(line), fpipe)) {
    std::string str(line);
    std::size_t found = str.find("Failed");
 
    if (found == std::string::npos) {
      std::vector<std::string> elm;
      elm = vpIoTools::splitChain(str, " ");
      for (size_t i = 0; i < elm.size(); i++) {
        if (!elm[i].empty()) {
          std::vector<std::string> resolution = vpIoTools::splitChain(elm[i], "x");
          if (resolution.size() == 2) {
            std::istringstream sswidth(resolution[0]), ssheight(resolution[1]);
            sswidth >> w;
            ssheight >> h;
            break;
          }
        }
      }
    }
  }
  pclose(fpipe);
#elif defined(_WIN32)
#if !defined(WINRT)
  w = GetSystemMetrics(SM_CXSCREEN);
  h = GetSystemMetrics(SM_CYSCREEN);
#else
  throw(vpException(vpException::functionNotImplementedError, "The function vpDisplayOpenCV::getScreenSize() is not "
                    "implemented on winrt"));
#endif
#endif
}
 
unsigned int vpDisplayOpenCV::getScreenWidth()
{
  unsigned int width, height;
  getScreenSize(width, height);
  return width;
}
 
unsigned int vpDisplayOpenCV::getScreenHeight()
{
  unsigned int width, height;
  getScreenSize(width, height);
  return height;
}
 
void vpDisplayOpenCV::overlay(std::function<void(cv::Mat &)> overlay_function, double opacity)
{
  // Initialize overlay layer for transparency
  cv::Mat overlay;
  if (opacity < 1.0) {
    // Deep copy
    overlay = m_background.clone();
  }
  else {
    // Shallow copy
    overlay = m_background;
  }
 
  overlay_function(overlay);
 
  // Blend background and overlay
  if (opacity < 1.0) {
    cv::addWeighted(overlay, opacity, m_background, 1.0 - opacity, 0.0, m_background);
  }
}
 
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_core.a(vpDisplayOpenCV.cpp.o) has no symbols
void dummy_vpDisplayOpenCV() { };
#endif