Visual Servoing Platform  version 3.2.0
vpWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Wire frame simulator
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #ifndef vpWireFrameSimulator_HH
40 #define vpWireFrameSimulator_HH
41 
46 #include <cmath> // std::fabs
47 #include <iostream>
48 #include <limits> // numeric_limits
49 #include <list>
50 #include <stdio.h>
51 #include <string>
52 #include <visp3/core/vpConfig.h>
53 
54 #include <visp3/core/vpConfig.h>
55 #include <visp3/core/vpDisplay.h>
56 #include <visp3/core/vpHomogeneousMatrix.h>
57 #include <visp3/core/vpImage.h>
58 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
59 #include <visp3/core/vpList.h>
60 #endif
61 #include <visp3/core/vpImagePoint.h>
62 #include <visp3/robot/vpImageSimulator.h>
63 #include <visp3/robot/vpWireFrameSimulatorTypes.h>
64 
155 class VISP_EXPORT vpWireFrameSimulator
156 {
157 public:
161  typedef enum {
162  THREE_PTS,
163  CUBE,
166  PLATE,
167  SMALL_PLATE,
170  RECTANGLE,
173  SQUARE_10CM,
177  DIAMOND,
181  TRAPEZOID,
184  THREE_LINES,
188  ROAD,
189  TIRE,
190  PIPE,
191  CIRCLE,
193  SPHERE,
194  CYLINDER,
195  PLAN,
196  POINT_CLOUD,
198  } vpSceneObject;
203 
212  typedef enum {
213  D_STANDARD,
214  D_CIRCLE,
216  D_TOOL
217  } vpSceneDesiredObject;
218 
219  typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
220 
221 protected:
222  Bound_scene scene;
223  Bound_scene desiredScene;
224  Bound_scene camera;
225  std::list<vpImageSimulator> objectImage;
226 
232  vpHomogeneousMatrix cdMo;
233 
235  vpSceneDesiredObject desiredObject;
236 
237  vpColor camColor;
238  vpColor camTrajColor;
239  vpColor curColor;
240  vpColor desColor;
241 
242  bool sceneInitialized;
243 
244  bool displayCameraTrajectory;
245  std::list<vpImagePoint> cameraTrajectory;
246  std::list<vpHomogeneousMatrix> poseList;
247  std::list<vpHomogeneousMatrix> fMoList;
248  unsigned int nbrPtLimit;
249 
250  vpImagePoint old_iPr;
251  vpImagePoint old_iPz;
252  vpImagePoint old_iPt;
253  bool blockedr;
254  bool blockedz;
255  bool blockedt;
256  bool blocked;
257 
259  vpHomogeneousMatrix f2Mf;
260 
261  double px_int;
262  double py_int;
263  double px_ext;
264  double py_ext;
265 
266  bool displayObject;
267  bool displayDesiredObject;
268  bool displayCamera;
269  bool displayImageSimulator;
270 
271  float cameraFactor;
272 
273  vpCameraTrajectoryDisplayType camTrajType;
274 
275  bool extCamChanged;
276 
277  vpHomogeneousMatrix rotz;
278 
279  unsigned int thickness_;
280 
281 private:
282  std::string scene_dir;
283 
284 public:
286  virtual ~vpWireFrameSimulator();
287 
294  inline void deleteCameraPositionHistory()
295  {
296  cameraTrajectory.clear();
297  poseList.clear();
298  fMoList.clear();
299  }
300 
301  void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
302  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
303  void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
304  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
305 
313  vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const
314  {
315  // if(px_ext != 1 && py_ext != 1)
316  // we assume px_ext and py_ext > 0
317  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
318  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
319  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
320  else {
321  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
322  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
323  }
324  }
332  vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const
333  {
334  // if(px_ext != 1 && py_ext != 1)
335  // we assume px_ext and py_ext > 0
336  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
337  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
338  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
339  else {
340  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
341  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
342  }
343  }
351  inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; }
352 
353  void getExternalImage(vpImage<unsigned char> &I);
354  void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
355  void getExternalImage(vpImage<vpRGBa> &I);
356  void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
357 
365  vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const
366  {
367  // if(px_int != 1 && py_int != 1)
368  // we assume px_int and py_int > 0
369  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
370  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
371  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
372  else {
373  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
374  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
375  }
376  }
384  vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const
385  {
386  // if(px_int != 1 && py_int != 1)
387  // we assume px_int and py_int > 0
388  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
389  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
390  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
391  else {
392  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
393  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
394  }
395  }
396 
397  void getInternalImage(vpImage<unsigned char> &I);
398  void getInternalImage(vpImage<vpRGBa> &I);
399 
405  vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }
406 
413  void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
414  {
415  cMo_history.clear();
416  for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
417  cMo_history.push_back(rotz * (*it));
418  }
419  }
420 
426  vpHomogeneousMatrix get_fMo() const { return fMo; }
427 
434  void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }
435 
436  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
437  void initScene(const char *obj, const char *desiredObject);
438  void initScene(const vpSceneObject &obj);
439  void initScene(const char *obj);
440 
441  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
442  const std::list<vpImageSimulator> &imObj);
443  void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
444  void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
445  void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);
446 
452  void setCameraColor(const vpColor &col) { camColor = col; }
458  void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
459  {
460  this->cMo = rotz * cMo_;
461  fMc = fMo * this->cMo.inverse();
462  }
463 
470  void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
471  {
472  this->fMc = fMc_ * rotz;
473  cMo = this->fMc.inverse() * fMo;
474  }
475 
482  inline void setCameraSizeFactor(const float factor) { cameraFactor = factor; }
483 
490  void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }
491 
499  inline void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
500  {
501  this->camTrajType = camTraj_type;
502  }
503 
509  void setCurrentViewColor(const vpColor &col) { curColor = col; }
515  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
521  void setDesiredViewColor(const vpColor &col) { desColor = col; }
530  void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }
531 
537  inline void setExternalCameraParameters(const vpCameraParameters &cam)
538  {
539  px_ext = cam.get_px();
540  py_ext = cam.get_py();
541  }
548  void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
549  {
550  this->camMf = rotz * cam_Mf;
552  this->camMf.extract(T);
553  this->camMf2.buildFrom(0, 0, T[2], 0, 0, 0);
554  f2Mf = camMf2.inverse() * this->camMf;
555  extCamChanged = true;
556  }
557 
561  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
562 
568  inline void setInternalCameraParameters(const vpCameraParameters &cam)
569  {
570  px_int = cam.get_px();
571  py_int = cam.get_py();
572  }
573 
581  inline void setNbPtTrajectory(const unsigned int nbPt) { nbrPtLimit = nbPt; }
582 
588  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ }
590 
591 protected:
594  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
595  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
596  vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
597  vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
598  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
599  const vpHomogeneousMatrix &fMo);
600  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
601  const vpHomogeneousMatrix &fMo);
602  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
603  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
604  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
605  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
607 };
608 
609 #endif
vpWireFrameSimulator::D_STANDARD
Definition: vpWireFrameSimulator.h:212
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:332
vpWireFrameSimulator::vpCameraTrajectoryDisplayType
vpCameraTrajectoryDisplayType
Definition: vpWireFrameSimulator.h:218
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpWireFrameSimulator::PLAN
Definition: vpWireFrameSimulator.h:194
vpWireFrameSimulator
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
Definition: vpWireFrameSimulator.h:154
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:90
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:329
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:552
vpWireFrameSimulator::vpSceneDesiredObject
vpSceneDesiredObject
Definition: vpWireFrameSimulator.h:211
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpMath::minimum
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:144
vpWireFrameSimulator::vpSceneObject
vpSceneObject
Definition: vpWireFrameSimulator.h:160
vpHomogeneousMatrix::buildFrom
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:222
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
vpWireFrameSimulator::PIPE
Definition: vpWireFrameSimulator.h:189
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:642
vpImage< unsigned char >
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpMath::maximum
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:136
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238