vpWireFrameSimulator.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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 <visp3/core/vpConfig.h>
#include <stdio.h>
#include <iostream>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#include <list>
#include <string>

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpDisplay.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 (const 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(const 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