Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
151class VISP_EXPORT vpWireFrameSimulator
152{
153public:
157 typedef enum {
158 THREE_PTS,
162 PLATE,
165 SMALL_PLATE,
168 RECTANGLE,
172 SQUARE_10CM,
176 DIAMOND,
179 TRAPEZOID,
186 PIPE,
191 PLAN,
193 POINT_CLOUD,
198 } vpSceneObject;
199
208 typedef enum {
209 D_STANDARD,
212 D_TOOL
213 } vpSceneDesiredObject;
214
215 typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
216
217protected:
218 Bound_scene scene;
219 Bound_scene desiredScene;
220 Bound_scene camera;
221 std::list<vpImageSimulator> objectImage;
222
229
232
237
239
241 std::list<vpImagePoint> cameraTrajectory;
242 std::list<vpHomogeneousMatrix> poseList;
243 std::list<vpHomogeneousMatrix> fMoList;
244 unsigned int nbrPtLimit;
245
253
256
257 double px_int;
258 double py_int;
259 double px_ext;
260 double py_ext;
261
266
268
270
272
274
275 unsigned int thickness_;
276
277private:
278 std::string scene_dir;
279
280public:
282 virtual ~vpWireFrameSimulator();
283
291 {
292 cameraTrajectory.clear();
293 poseList.clear();
294 fMoList.clear();
295 }
296
297 void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
298 const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
299 void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
300 const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
301
310 {
311 // if(px_ext != 1 && py_ext != 1)
312 // we assume px_ext and py_ext > 0
313 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
314 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
315 return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
316 else {
317 unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
318 return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
319 }
320 }
329 {
330 // if(px_ext != 1 && py_ext != 1)
331 // we assume px_ext and py_ext > 0
332 if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
333 (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
334 return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
335 else {
336 unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
337 return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
338 }
339 }
347 inline vpHomogeneousMatrix getExternalCameraPosition() const { return rotz * camMf; }
348
349 void getExternalImage(vpImage<unsigned char> &I);
350 void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
351 void getExternalImage(vpImage<vpRGBa> &I);
352 void getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &camMf);
353
362 {
363 // if(px_int != 1 && py_int != 1)
364 // we assume px_int and py_int > 0
365 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
366 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
367 return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
368 else {
369 unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
370 return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
371 }
372 }
381 {
382 // if(px_int != 1 && py_int != 1)
383 // we assume px_int and py_int > 0
384 if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
385 (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
386 return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
387 else {
388 unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
389 return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
390 }
391 }
392
393 void getInternalImage(vpImage<unsigned char> &I);
394 void getInternalImage(vpImage<vpRGBa> &I);
395
401 vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }
402
409 void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
410 {
411 cMo_history.clear();
412 for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
413 cMo_history.push_back(rotz * (*it));
414 }
415 }
416
422 vpHomogeneousMatrix get_fMo() const { return fMo; }
423
430 void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }
431
432 void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
433 void initScene(const char *obj, const char *desiredObject);
434 void initScene(const vpSceneObject &obj);
435 void initScene(const char *obj);
436
437 void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
438 const std::list<vpImageSimulator> &imObj);
439 void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
440 void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
441 void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);
442
448 void setCameraColor(const vpColor &col) { camColor = col; }
455 {
456 this->cMo = rotz * cMo_;
457 fMc = fMo * this->cMo.inverse();
458 }
459
467 {
468 this->fMc = fMc_ * rotz;
469 cMo = this->fMc.inverse() * fMo;
470 }
471
478 inline void setCameraSizeFactor(float factor) { cameraFactor = factor; }
479
486 void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }
487
496 {
497 this->camTrajType = camTraj_type;
498 }
499
505 void setCurrentViewColor(const vpColor &col) { curColor = col; }
511 void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
517 void setDesiredViewColor(const vpColor &col) { desColor = col; }
526 void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }
527
534 {
535 px_ext = cam.get_px();
536 py_ext = cam.get_py();
537 }
545 {
546 this->camMf = rotz * cam_Mf;
548 this->camMf.extract(T);
549 this->camMf2.buildFrom(0, 0, T[2], 0, 0, 0);
550 f2Mf = camMf2.inverse() * this->camMf;
551 extCamChanged = true;
552 }
553
557 void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
558
565 {
566 px_int = cam.get_px();
567 py_int = cam.get_py();
568 }
569
577 inline void setNbPtTrajectory(unsigned int nbPt) { nbrPtLimit = nbPt; }
578
584 void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; /*this->cMo = fMc.inverse()*fMo;*/ }
586
587protected:
590 void display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color);
591 void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
592 vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
593 vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
594 vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
595 const vpHomogeneousMatrix &fMo);
596 vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
597 const vpHomogeneousMatrix &fMo);
598 vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
599 const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
600 vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
601 const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
603};
604
605#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 inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
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