Visual Servoing Platform  version 3.4.0 under development (2021-03-03)
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 
53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDisplay.h>
55 #include <visp3/core/vpHomogeneousMatrix.h>
56 #include <visp3/core/vpImage.h>
57 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
58 #include <visp3/core/vpList.h>
59 #endif
60 #include <visp3/core/vpImagePoint.h>
61 #include <visp3/robot/vpImageSimulator.h>
62 #include <visp3/robot/vpWireFrameSimulatorTypes.h>
63 
154 class VISP_EXPORT vpWireFrameSimulator
155 {
156 public:
160  typedef enum {
161  THREE_PTS,
162  CUBE,
165  PLATE,
166  SMALL_PLATE,
169  RECTANGLE,
172  SQUARE_10CM,
176  DIAMOND,
180  TRAPEZOID,
183  THREE_LINES,
189  PIPE,
190  CIRCLE,
194  PLAN,
195  POINT_CLOUD,
197  } vpSceneObject;
202 
211  typedef enum {
212  D_STANDARD,
213  D_CIRCLE,
215  D_TOOL
217 
218  typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
219 
220 protected:
221  Bound_scene scene;
222  Bound_scene desiredScene;
223  Bound_scene camera;
224  std::list<vpImageSimulator> objectImage;
225 
232 
233  vpSceneObject object;
234  vpSceneDesiredObject desiredObject;
235 
240 
242 
244  std::list<vpImagePoint> cameraTrajectory;
245  std::list<vpHomogeneousMatrix> poseList;
246  std::list<vpHomogeneousMatrix> fMoList;
247  unsigned int nbrPtLimit;
248 
252  bool blockedr;
253  bool blockedz;
254  bool blockedt;
255  bool blocked;
256 
259 
260  double px_int;
261  double py_int;
262  double px_ext;
263  double py_ext;
264 
269 
271 
272  vpCameraTrajectoryDisplayType camTrajType;
273 
275 
277 
278  unsigned int thickness_;
279 
280 private:
281  std::string scene_dir;
282 
283 public:
285  virtual ~vpWireFrameSimulator();
286 
294  {
295  cameraTrajectory.clear();
296  poseList.clear();
297  fMoList.clear();
298  }
299 
300  void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
301  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
302  void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
303  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
304 
313  {
314  // if(px_ext != 1 && py_ext != 1)
315  // we assume px_ext and py_ext > 0
316  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
317  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
318  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
319  else {
320  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
321  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
322  }
323  }
332  {
333  // if(px_ext != 1 && py_ext != 1)
334  // we assume px_ext and py_ext > 0
335  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
336  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
337  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
338  else {
339  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
340  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
341  }
342  }
350  inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; }
351 
352  void getExternalImage(vpImage<unsigned char> &I);
353  void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
354  void getExternalImage(vpImage<vpRGBa> &I);
355  void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
356 
365  {
366  // if(px_int != 1 && py_int != 1)
367  // we assume px_int and py_int > 0
368  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
369  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
370  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
371  else {
372  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
373  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
374  }
375  }
384  {
385  // if(px_int != 1 && py_int != 1)
386  // we assume px_int and py_int > 0
387  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
388  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
389  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
390  else {
391  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
392  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
393  }
394  }
395 
396  void getInternalImage(vpImage<unsigned char> &I);
397  void getInternalImage(vpImage<vpRGBa> &I);
398 
404  vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }
405 
412  void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
413  {
414  cMo_history.clear();
415  for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
416  cMo_history.push_back(rotz * (*it));
417  }
418  }
419 
425  vpHomogeneousMatrix get_fMo() const { return fMo; }
426 
433  void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }
434 
435  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
436  void initScene(const char *obj, const char *desiredObject);
437  void initScene(const vpSceneObject &obj);
438  void initScene(const char *obj);
439 
440  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
441  const std::list<vpImageSimulator> &imObj);
442  void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
443  void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
444  void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);
445 
451  void setCameraColor(const vpColor &col) { camColor = col; }
458  {
459  this->cMo = rotz * cMo_;
460  fMc = fMo * this->cMo.inverse();
461  }
462 
470  {
471  this->fMc = fMc_ * rotz;
472  cMo = this->fMc.inverse() * fMo;
473  }
474 
481  inline void setCameraSizeFactor(float factor) { cameraFactor = factor; }
482 
489  void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }
490 
498  inline void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
499  {
500  this->camTrajType = camTraj_type;
501  }
502 
508  void setCurrentViewColor(const vpColor &col) { curColor = col; }
514  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
520  void setDesiredViewColor(const vpColor &col) { desColor = col; }
529  void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }
530 
537  {
538  px_ext = cam.get_px();
539  py_ext = cam.get_py();
540  }
548  {
549  this->camMf = rotz * cam_Mf;
551  this->camMf.extract(T);
552  this->camMf2.buildFrom(0, 0, T[2], 0, 0, 0);
553  f2Mf = camMf2.inverse() * this->camMf;
554  extCamChanged = true;
555  }
556 
560  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
561 
568  {
569  px_int = cam.get_px();
570  py_int = cam.get_py();
571  }
572 
580  inline void setNbPtTrajectory(unsigned int nbPt) { nbrPtLimit = nbPt; }
581 
587  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ }
589 
590 protected:
593  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
594  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
595  vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
596  vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
597  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
598  const vpHomogeneousMatrix &fMo);
599  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
600  const vpHomogeneousMatrix &fMo);
601  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
602  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
603  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
604  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
606 };
607 
608 #endif
A cylinder of 80cm length and 10cm radius.
A tire represented by 2 circles with radius 10cm and 15cm.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMo() const
void setCameraColor(const vpColor &col)
void setDesiredViewColor(const vpColor &col)
void set_fMo(const vpHomogeneousMatrix &fMo_)
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void get_cMo_History(std::list< vpHomogeneousMatrix > &cMo_history)
void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
vpCameraParameters getExternalCameraParameters(const vpImage< vpRGBa > &I) const
std::list< vpImageSimulator > objectImage
void setCurrentViewColor(const vpColor &col)
std::list< vpImagePoint > cameraTrajectory
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix cdMo
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void extract(vpRotationMatrix &R) const
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:145
void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
void setCameraTrajectoryColor(const vpColor &col)
vpHomogeneousMatrix get_cMo() const
vpHomogeneousMatrix f2Mf
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix cMo
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setExternalCameraParameters(const vpCameraParameters &cam)
void setCameraSizeFactor(float factor)
vpCameraTrajectoryDisplayType camTrajType
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
vpSceneDesiredObject desiredObject
vpHomogeneousMatrix camMf
Three parallel lines representing a road.
vpHomogeneousMatrix fMc
unsigned int getHeight() const
Definition: vpImage.h:188
vpHomogeneousMatrix rotz
void setInternalCameraParameters(const vpCameraParameters &cam)
void setDisplayCameraTrajectory(const bool &do_display)
void get_fMo_History(std::list< vpHomogeneousMatrix > &fMo_history)
vpHomogeneousMatrix refMo
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
unsigned int getWidth() const
Definition: vpImage.h:246
Class that consider the case of a translation vector.
std::list< vpHomogeneousMatrix > poseList
vpCameraParameters getInternalCameraParameters(const vpImage< vpRGBa > &I) const
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix getExternalCameraPosition() const
void setNbPtTrajectory(unsigned int nbPt)