Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
vpProjectionDisplay.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface with the image for feature display.
32  *
33 *****************************************************************************/
34 
40 #include <visp3/core/vpConfig.h>
41 #if defined(VISP_HAVE_DISPLAY)
42 
43 // Meter/pixel conversion
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPoint.h>
48 
49 // Color / image / display
50 #include <visp3/core/vpColor.h>
51 #include <visp3/core/vpImage.h>
52 
53 #include <visp3/core/vpDisplay.h>
54 
55 #include <visp3/gui/vpProjectionDisplay.h>
56 
57 //#include <visp3/visual_features/vpBasicFeature.h>
58 
59 BEGIN_VISP_NAMESPACE
60 
62 {
63  // vpForwardProjection *f ;
64  // f = fp.duplicate() ;
65  // f->setDeallocate(vpForwardProjection::vpDisplayForwardProjection) ;
66 
67  listFp.push_back(&fp);
68 }
69 
71 {
72  o.setWorldCoordinates(0, 0, 0);
73  x.setWorldCoordinates(0.1, 0, 0);
74  y.setWorldCoordinates(0, 0.1, 0);
75  z.setWorldCoordinates(0, 0, 0.1);
76  traj.resize(0, 2);
77 }
78 void vpProjectionDisplay::init(const int select)
79 {
80  if (select & vpProjectionDisplay::internalView()) {
81  Icam.resize(256, 256);
82  dIcam.init(Icam, 100, 100);
83  }
84  if (select & vpProjectionDisplay::externalView()) {
85  Iext.resize(256, 256);
86  dIext.init(Iext, 400, 100);
87  }
88 
89  init();
90 }
91 
93 
95  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color,
96  const bool &displayTraj, unsigned int thickness)
97 {
98 
99  for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin(); it != listFp.end(); ++it) {
100  vpForwardProjection *fp = *it;
101  fp->display(I, cextMo, cam, color, thickness);
102  }
103 
104  if (displayTraj) // display past camera positions
105  for (unsigned int i = 0; i < traj.getRows(); ++i)
106  vpDisplay::displayCircle(I, static_cast<int>(traj[i][0]), static_cast<int>(traj[i][1]), 2, vpColor::green, true);
107 
108  displayCamera(I, cextMo, cMo, cam, thickness);
109 
110  if (displayTraj) // store current camera position
111  {
112  const unsigned int n = traj.getRows();
113  traj.resize(n + 1, 2, false);
114  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], traj[n][1], traj[n][0]);
115  }
116 }
117 
119  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
120  unsigned int thickness)
121 {
122  vpHomogeneousMatrix c1Mc;
123  c1Mc = cextMo * cMo.inverse();
124 
125  o.track(c1Mc);
126 
127  if (o.get_Z() < 0) // do not print camera if behind the external camera
128  return;
129 
130  x.track(c1Mc);
131  y.track(c1Mc);
132  z.track(c1Mc);
133 
134  vpImagePoint ipo;
135  vpImagePoint ip;
136 
137  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
138 
139  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip);
140  vpDisplay::displayArrow(I, ipo, ip, vpColor::red, 4 + thickness, 2 + thickness, thickness);
141 
142  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip);
143  vpDisplay::displayArrow(I, ipo, ip, vpColor::green, 4 + thickness, 2 + thickness, thickness);
144 
145  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip);
146  vpDisplay::displayArrow(I, ipo, ip, vpColor::blue, 4 + thickness, 2 + thickness, thickness);
147 }
148 
149 END_VISP_NAMESPACE
150 
151 #elif !defined(VISP_BUILD_SHARED_LIBS)
152 // Work around to avoid warning: libvisp_gui.a(vpProjectionDisplay.cpp.o)
153 // has no symbols
154 void dummy_vpProjectionDisplay() { };
155 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") VP_OVERRIDE
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void displayArrow(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)
Class that defines what is a generic geometric feature.
virtual void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)=0
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:542
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:111
void insert(vpForwardProjection &fp)
void displayCamera(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, unsigned int thickness=1)
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
vpColVector p
Definition: vpTracker.h:69