vpWireFrameSimulator.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 http://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:
 * Wire frame simulator
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#ifndef vpWireFrameSimulator_HH
#define vpWireFrameSimulator_HH

#include <cmath> // std::fabs
#include <iostream>
#include <limits> // numeric_limits
#include <list>
#include <stdio.h>
#include <string>

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
#include <visp3/core/vpList.h>
#endif
#include <visp3/core/vpImagePoint.h>
#include <visp3/robot/vpImageSimulator.h>
#include <visp3/robot/vpWireFrameSimulatorTypes.h>

class VISP_EXPORT vpWireFrameSimulator
{
public:
  typedef enum {
    THREE_PTS,   
    CUBE,        
    PLATE,       
    SMALL_PLATE, 
    RECTANGLE,   
    SQUARE_10CM, 
    DIAMOND,     
    TRAPEZOID,   
    THREE_LINES, 
    ROAD,        
    TIRE,        
    PIPE,        
    CIRCLE,      
    SPHERE,      
    CYLINDER,    
    PLAN,        
    POINT_CLOUD, 
  } vpSceneObject;

  typedef enum {
    D_STANDARD, 
    D_CIRCLE,   
    D_TOOL      
  } vpSceneDesiredObject;

  typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;

protected:
  Bound_scene scene;
  Bound_scene desiredScene;
  Bound_scene camera;
  std::list<vpImageSimulator> objectImage;

  vpHomogeneousMatrix fMo;
  vpHomogeneousMatrix fMc;
  vpHomogeneousMatrix camMf;
  vpHomogeneousMatrix refMo;
  vpHomogeneousMatrix cMo;
  vpHomogeneousMatrix cdMo;

  vpSceneObject object;
  vpSceneDesiredObject desiredObject;

  vpColor camColor;
  vpColor camTrajColor;
  vpColor curColor;
  vpColor desColor;

  bool sceneInitialized;

  bool displayCameraTrajectory;
  std::list<vpImagePoint> cameraTrajectory;
  std::list<vpHomogeneousMatrix> poseList;
  std::list<vpHomogeneousMatrix> fMoList;
  unsigned int nbrPtLimit;

  vpImagePoint old_iPr;
  vpImagePoint old_iPz;
  vpImagePoint old_iPt;
  bool blockedr;
  bool blockedz;
  bool blockedt;
  bool blocked;

  vpHomogeneousMatrix camMf2;
  vpHomogeneousMatrix f2Mf;

  double px_int;
  double py_int;
  double px_ext;
  double py_ext;

  bool displayObject;
  bool displayDesiredObject;
  bool displayCamera;
  bool displayImageSimulator;

  float cameraFactor;

  vpCameraTrajectoryDisplayType camTrajType;

  bool extCamChanged;

  vpHomogeneousMatrix rotz;

  unsigned int thickness_;

private:
  std::string scene_dir;

public:
  vpWireFrameSimulator();
  virtual ~vpWireFrameSimulator();

  inline void deleteCameraPositionHistory()
  {
    cameraTrajectory.clear();
    poseList.clear();
    fMoList.clear();
  }

  void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
                         const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
  void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
                         const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);

  vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const
  {
    // if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }
  vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const
  {
    // if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }
  inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; }

  void getExternalImage(vpImage<unsigned char> &I);
  void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
  void getExternalImage(vpImage<vpRGBa> &I);
  void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);

  vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const
  {
    // if(px_int != 1 && py_int != 1)
    // we assume px_int and py_int > 0
    if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }
  vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const
  {
    // if(px_int != 1 && py_int != 1)
    // we assume px_int and py_int > 0
    if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }

  void getInternalImage(vpImage<unsigned char> &I);
  void getInternalImage(vpImage<vpRGBa> &I);

  vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }

  void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
  {
    cMo_history.clear();
    for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
      cMo_history.push_back(rotz * (*it));
    }
  }

  vpHomogeneousMatrix get_fMo() const { return fMo; }

  void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }

  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
  void initScene(const char *obj, const char *desiredObject);
  void initScene(const vpSceneObject &obj);
  void initScene(const char *obj);

  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
                 const std::list<vpImageSimulator> &imObj);
  void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
  void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
  void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);

  void setCameraColor(const vpColor &col) { camColor = col; }
  void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
  {
    this->cMo = rotz * cMo_;
    fMc = fMo * this->cMo.inverse();
  }

  void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
  {
    this->fMc = fMc_ * rotz;
    cMo = this->fMc.inverse() * fMo;
  }

  inline void setCameraSizeFactor(float factor) { cameraFactor = factor; }

  void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }

  inline void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
  {
    this->camTrajType = camTraj_type;
  }

  void setCurrentViewColor(const vpColor &col) { curColor = col; }
  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
  void setDesiredViewColor(const vpColor &col) { desColor = col; }
  void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }

  inline void setExternalCameraParameters(const vpCameraParameters &cam)
  {
    px_ext = cam.get_px();
    py_ext = cam.get_py();
  }
  void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
  {
    this->camMf = rotz * cam_Mf;
    vpTranslationVector T;
    this->camMf.extract(T);
    this->camMf2.buildFrom(0, 0, T[2], 0, 0, 0);
    f2Mf = camMf2.inverse() * this->camMf;
    extCamChanged = true;
  }

  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }

  inline void setInternalCameraParameters(const vpCameraParameters &cam)
  {
    px_int = cam.get_px();
    py_int = cam.get_py();
  }

  inline void setNbPtTrajectory(unsigned int nbPt) { nbrPtLimit = nbPt; }

  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ }

protected:
  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
  vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
  vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                                       const vpHomogeneousMatrix &fMo);
  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                       const vpHomogeneousMatrix &fMo);
  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                                       const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                       const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
};

#endif