Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpProjectionDisplay.cpp
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  * Interface with the image for feature display.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpConfig.h>
45 #if defined(VISP_HAVE_DISPLAY)
46 
47 // Meter/pixel conversion
48 #include <visp3/core/vpCameraParameters.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMeterPixelConversion.h>
51 #include <visp3/core/vpPoint.h>
52 
53 // Color / image / display
54 #include <visp3/core/vpColor.h>
55 #include <visp3/core/vpImage.h>
56 
57 #include <visp3/core/vpDisplay.h>
58 
59 #include <visp3/gui/vpProjectionDisplay.h>
60 
61 //#include <visp3/visual_features/vpBasicFeature.h>
62 
64 {
65  // vpForwardProjection *f ;
66  // f = fp.duplicate() ;
67  // f->setDeallocate(vpForwardProjection::vpDisplayForwardProjection) ;
68 
69  listFp.push_back(&fp);
70 }
71 
73 {
74  o.setWorldCoordinates(0, 0, 0);
75  x.setWorldCoordinates(0.1, 0, 0);
76  y.setWorldCoordinates(0, 0.1, 0);
77  z.setWorldCoordinates(0, 0, 0.1);
78  traj.resize(0, 2);
79 }
80 void vpProjectionDisplay::init(const int select)
81 {
82  if (select & vpProjectionDisplay::internalView()) {
83  Icam.resize(256, 256);
84  dIcam.init(Icam, 100, 100);
85  }
86  if (select & vpProjectionDisplay::externalView()) {
87  Iext.resize(256, 256);
88  dIext.init(Iext, 400, 100);
89  }
90 
91  init();
92 }
93 
95 
97  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color,
98  const bool &displayTraj, const unsigned int thickness)
99 {
100 
101  for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin(); it != listFp.end(); ++it) {
102  vpForwardProjection *fp = *it;
103  fp->display(I, cextMo, cam, color, thickness);
104  }
105 
106  if (displayTraj) // display past camera positions
107  for (unsigned int i = 0; i < traj.getRows(); ++i)
108  vpDisplay::displayCircle(I, (int)traj[i][0], (int)traj[i][1], 2, vpColor::green, true);
109 
110  displayCamera(I, cextMo, cMo, cam, thickness);
111 
112  if (displayTraj) // store current camera position
113  {
114  const unsigned int n = traj.getRows();
115  traj.resize(n + 1, 2, false);
116  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], traj[n][1], traj[n][0]);
117  }
118 }
119 
121  const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
122  const unsigned int thickness)
123 {
124  vpHomogeneousMatrix c1Mc;
125  c1Mc = cextMo * cMo.inverse();
126 
127  o.track(c1Mc);
128 
129  if (o.get_Z() < 0) // do not print camera if behind the external camera
130  return;
131 
132  x.track(c1Mc);
133  y.track(c1Mc);
134  z.track(c1Mc);
135 
136  vpImagePoint ipo;
137  vpImagePoint ipx;
138 
139  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
140 
141  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx);
142  vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4 + thickness, 2 + thickness, thickness);
143 
144  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx);
145  vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4 + thickness, 2 + thickness, thickness);
146 
147  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx);
148  vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4 + thickness, 2 + thickness, thickness);
149 }
150 
151 #elif !defined(VISP_BUILD_SHARED_LIBS)
152 // Work arround to avoid warning: libvisp_core.a(vpProjectionDisplay.cpp.o)
153 // has no symbols
154 void dummy_vpProjectionDisplay(){};
155 #endif
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Definition: vpDisplayX.cpp:258
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, const unsigned int thickness=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
virtual void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)=0
void track(const vpHomogeneousMatrix &cMo)
static const vpColor green
Definition: vpColor.h:183
static const vpColor red
Definition: vpColor.h:180
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.
Generic class defining intrinsic camera parameters.
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:863
void insert(vpForwardProjection &fp)
unsigned int getRows() const
Definition: vpArray2D.h:289
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:415
static void displayCircle(const vpImage< unsigned char > &I, const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
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:88
void displayCamera(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const unsigned int thickness=1)
vpColVector p
Definition: vpTracker.h:71
static const vpColor blue
Definition: vpColor.h:186