Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
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,
190  PIPE,
191  CIRCLE,
195  PLAN,
196  POINT_CLOUD,
198  } vpSceneObject;
203 
212  typedef enum {
213  D_STANDARD,
214  D_CIRCLE,
216  D_TOOL
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 
233 
234  vpSceneObject object;
235  vpSceneDesiredObject desiredObject;
236 
241 
243 
245  std::list<vpImagePoint> cameraTrajectory;
246  std::list<vpHomogeneousMatrix> poseList;
247  std::list<vpHomogeneousMatrix> fMoList;
248  unsigned int nbrPtLimit;
249 
253  bool blockedr;
254  bool blockedz;
255  bool blockedt;
256  bool blocked;
257 
260 
261  double px_int;
262  double py_int;
263  double px_ext;
264  double py_ext;
265 
270 
272 
273  vpCameraTrajectoryDisplayType camTrajType;
274 
276 
278 
279  unsigned int thickness_;
280 
281 private:
282  std::string scene_dir;
283 
284 public:
286  virtual ~vpWireFrameSimulator();
287 
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 
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  }
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 
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  }
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; }
459  {
460  this->cMo = rotz * cMo_;
461  fMc = fMo * this->cMo.inverse();
462  }
463 
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 
538  {
539  px_ext = cam.get_px();
540  py_ext = cam.get_py();
541  }
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 
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
A cylinder of 80cm length and 10cm radius.
A tire represented by 2 circles with radius 10cm and 15cm.
vpCameraParameters getInternalCameraParameters(const vpImage< vpRGBa > &I) const
unsigned int getWidth() const
Definition: vpImage.h:237
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setCameraColor(const vpColor &col)
void setDesiredViewColor(const vpColor &col)
void set_fMo(const vpHomogeneousMatrix &fMo_)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void setNbPtTrajectory(const unsigned int nbPt)
void get_cMo_History(std::list< vpHomogeneousMatrix > &cMo_history)
void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
std::list< vpImageSimulator > objectImage
void setCurrentViewColor(const vpColor &col)
std::list< vpImagePoint > cameraTrajectory
vpHomogeneousMatrix cdMo
double get_py() const
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
vpHomogeneousMatrix fMo
void setGraphicsThickness(unsigned int thickness)
std::list< vpHomogeneousMatrix > fMoList
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
void setCameraTrajectoryColor(const vpColor &col)
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
vpHomogeneousMatrix get_fMo() const
vpHomogeneousMatrix f2Mf
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix cMo
void extract(vpRotationMatrix &R) const
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double get_px() const
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
void setExternalCameraParameters(const vpCameraParameters &cam)
vpCameraTrajectoryDisplayType camTrajType
vpCameraParameters getExternalCameraParameters(const vpImage< vpRGBa > &I) const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
vpSceneDesiredObject desiredObject
vpHomogeneousMatrix camMf
Three parallel lines representing a road.
vpHomogeneousMatrix fMc
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix get_cMo() const
vpHomogeneousMatrix rotz
void setInternalCameraParameters(const vpCameraParameters &cam)
void setDisplayCameraTrajectory(const bool &do_display)
void get_fMo_History(std::list< vpHomogeneousMatrix > &fMo_history)
vpHomogeneousMatrix refMo
unsigned int getHeight() const
Definition: vpImage.h:179
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
Class that consider the case of a translation vector.
void setCameraSizeFactor(const float factor)
std::list< vpHomogeneousMatrix > poseList
vpHomogeneousMatrix getExternalCameraPosition() const
vpHomogeneousMatrix camMf2