Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
vpWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #ifndef vpWireFrameSimulator_HH
37 #define vpWireFrameSimulator_HH
38 
43 #include <cmath> // std::fabs
44 #include <iostream>
45 #include <limits> // numeric_limits
46 #include <list>
47 #include <stdio.h>
48 #include <string>
49 
50 #include <visp3/core/vpConfig.h>
51 #include <visp3/core/vpDisplay.h>
52 #include <visp3/core/vpHomogeneousMatrix.h>
53 #include <visp3/core/vpImage.h>
54 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
55 #include <visp3/core/vpList.h>
56 #endif
57 #include <visp3/core/vpImagePoint.h>
58 #include <visp3/robot/vpImageSimulator.h>
59 #include <visp3/robot/vpWireFrameSimulatorTypes.h>
60 
151 class VISP_EXPORT vpWireFrameSimulator
152 {
153 public:
157  typedef enum
158  {
159  THREE_PTS,
163  PLATE,
166  SMALL_PLATE,
169  RECTANGLE,
173  SQUARE_10CM,
177  DIAMOND,
180  TRAPEZOID,
187  PIPE,
192  PLAN,
194  POINT_CLOUD,
199  } vpSceneObject;
200 
209  typedef enum
210  {
211  D_STANDARD,
214  D_TOOL
215  } vpSceneDesiredObject;
216 
217  typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
218 
219 protected:
220  Bound_scene scene;
221  Bound_scene desiredScene;
222  Bound_scene camera;
223  std::list<vpImageSimulator> objectImage;
224 
231 
234 
239 
241 
243  std::list<vpImagePoint> cameraTrajectory;
244  std::list<vpHomogeneousMatrix> poseList;
245  std::list<vpHomogeneousMatrix> fMoList;
246  unsigned int nbrPtLimit;
247 
251  bool blockedr;
252  bool blockedz;
253  bool blockedt;
254  bool blocked;
255 
258 
259  double px_int;
260  double py_int;
261  double px_ext;
262  double py_ext;
263 
268 
270 
272 
274 
276 
277  unsigned int thickness_;
278 
279 private:
280  std::string scene_dir;
281 
282 public:
284  virtual ~vpWireFrameSimulator();
285 
293  {
294  cameraTrajectory.clear();
295  poseList.clear();
296  fMoList.clear();
297  }
298 
299  void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
300  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
301  void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
302  const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
303 
312  {
313  // if(px_ext != 1 && py_ext != 1)
314  // we assume px_ext and py_ext > 0
315  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
316  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
317  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
318  else {
319  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
320  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
321  }
322  }
331  {
332  // if(px_ext != 1 && py_ext != 1)
333  // we assume px_ext and py_ext > 0
334  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
335  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
336  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
337  else {
338  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
339  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
340  }
341  }
349  inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; }
350 
351  void getExternalImage(vpImage<unsigned char> &I);
352  void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
353  void getExternalImage(vpImage<vpRGBa> &I);
354  void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
355 
364  {
365  // if(px_int != 1 && py_int != 1)
366  // we assume px_int and py_int > 0
367  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
368  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
369  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
370  else {
371  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
372  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
373  }
374  }
383  {
384  // if(px_int != 1 && py_int != 1)
385  // we assume px_int and py_int > 0
386  if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
387  (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
388  return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
389  else {
390  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
391  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
392  }
393  }
394 
395  void getInternalImage(vpImage<unsigned char> &I);
396  void getInternalImage(vpImage<vpRGBa> &I);
397 
403  vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }
404 
411  void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
412  {
413  cMo_history.clear();
414  for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
415  cMo_history.push_back(rotz * (*it));
416  }
417  }
418 
424  vpHomogeneousMatrix get_fMo() const { return fMo; }
425 
432  void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }
433 
434  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
435  void initScene(const char *obj, const char *desiredObject);
436  void initScene(const vpSceneObject &obj);
437  void initScene(const char *obj);
438 
439  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
440  const std::list<vpImageSimulator> &imObj);
441  void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
442  void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
443  void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);
444 
450  void setCameraColor(const vpColor &col) { camColor = col; }
457  {
458  this->cMo = rotz * cMo_;
459  fMc = fMo * this->cMo.inverse();
460  }
461 
469  {
470  this->fMc = fMc_ * rotz;
471  cMo = this->fMc.inverse() * fMo;
472  }
473 
480  inline void setCameraSizeFactor(float factor) { cameraFactor = factor; }
481 
488  void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }
489 
498  {
499  this->camTrajType = camTraj_type;
500  }
501 
507  void setCurrentViewColor(const vpColor &col) { curColor = col; }
513  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
519  void setDesiredViewColor(const vpColor &col) { desColor = col; }
528  void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }
529 
536  {
537  px_ext = cam.get_px();
538  py_ext = cam.get_py();
539  }
547  {
548  this->camMf = rotz * cam_Mf;
550  this->camMf.extract(T);
551  this->camMf2.build(0, 0, T[2], 0, 0, 0);
552  f2Mf = camMf2.inverse() * this->camMf;
553  extCamChanged = true;
554  }
555 
559  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
560 
567  {
568  px_int = cam.get_px();
569  py_int = cam.get_py();
570  }
571 
579  inline void setNbPtTrajectory(unsigned int nbPt) { nbrPtLimit = nbPt; }
580 
586  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ }
588 
589 protected:
592  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
593  void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
594  vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
595  vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
596  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
597  const vpHomogeneousMatrix &fMo);
598  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
599  const vpHomogeneousMatrix &fMo);
600  vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
601  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
602  vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
603  const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
605 };
606 
607 #endif
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
Class that consider the case of a translation vector.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
vpSceneDesiredObject desiredObject
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
@ D_CIRCLE
The object displayed at the desired position is a circle.
vpHomogeneousMatrix refMo
vpCameraParameters getInternalCameraParameters(const vpImage< unsigned char > &I) const
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void get_cMo_History(std::list< vpHomogeneousMatrix > &cMo_history)
vpCameraParameters getExternalCameraParameters(const vpImage< vpRGBa > &I) const
void get_fMo_History(std::list< vpHomogeneousMatrix > &fMo_history)
vpCameraTrajectoryDisplayType camTrajType
void setCurrentViewColor(const vpColor &col)
void setNbPtTrajectory(unsigned int nbPt)
void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
vpHomogeneousMatrix get_cMo() const
vpCameraParameters getExternalCameraParameters(const vpImage< unsigned char > &I) const
vpHomogeneousMatrix fMo
void setCameraColor(const vpColor &col)
void setDesiredViewColor(const vpColor &col)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix fMc
vpCameraParameters getInternalCameraParameters(const vpImage< vpRGBa > &I) const
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setCameraTrajectoryColor(const vpColor &col)
std::list< vpImagePoint > cameraTrajectory
vpHomogeneousMatrix cMo
std::list< vpHomogeneousMatrix > fMoList
void setCameraSizeFactor(float factor)
void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
std::list< vpImageSimulator > objectImage
void set_fMo(const vpHomogeneousMatrix &fMo_)
vpHomogeneousMatrix get_fMo() const
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
@ CIRCLE
A 10cm radius circle.
@ THREE_LINES
Three parallel lines with equation y=-5, y=0, y=5.
@ ROAD
Three parallel lines representing a road.
@ SPHERE
A 15cm radius sphere.
@ CUBE
A 12.5cm size cube.
@ TIRE
A tire represented by 2 circles with radius 10cm and 15cm.
@ CYLINDER
A cylinder of 80cm length and 10cm radius.
vpHomogeneousMatrix rotz
void setGraphicsThickness(unsigned int thickness)
vpHomogeneousMatrix cdMo
void setDisplayCameraTrajectory(const bool &do_display)
std::list< vpHomogeneousMatrix > poseList