Visual Servoing Platform version 3.5.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
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
154class VISP_EXPORT vpWireFrameSimulator
155{
156public:
160 typedef enum {
161 THREE_PTS,
165 PLATE,
168 SMALL_PLATE,
171 RECTANGLE,
175 SQUARE_10CM,
179 DIAMOND,
182 TRAPEZOID,
189 PIPE,
194 PLAN,
196 POINT_CLOUD,
201 } vpSceneObject;
202
211 typedef enum {
212 D_STANDARD,
215 D_TOOL
216 } vpSceneDesiredObject;
217
218 typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
219
220protected:
221 Bound_scene scene;
222 Bound_scene desiredScene;
223 Bound_scene camera;
224 std::list<vpImageSimulator> objectImage;
225
232
235
240
242
244 std::list<vpImagePoint> cameraTrajectory;
245 std::list<vpHomogeneousMatrix> poseList;
246 std::list<vpHomogeneousMatrix> fMoList;
247 unsigned int nbrPtLimit;
248
256
259
260 double px_int;
261 double py_int;
262 double px_ext;
263 double py_ext;
264
269
271
273
275
277
278 unsigned int thickness_;
279
280private:
281 std::string scene_dir;
282
283public:
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
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
590protected:
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
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
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