Visual Servoing Platform  version 3.0.0
vpProjectionDisplay.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  *
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/vpMeterPixelConversion.h>
50 #include <visp3/core/vpPoint.h>
51 #include <visp3/core/vpMath.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 
63 
64 void
66 {
67  // vpForwardProjection *f ;
68  // f = fp.duplicate() ;
69  // f->setDeallocate(vpForwardProjection::vpDisplayForwardProjection) ;
70 
71  listFp.push_back(&fp);
72 }
73 
74 void
76 {
77  o.setWorldCoordinates(0,0,0) ;
78  x.setWorldCoordinates(0.1,0,0) ;
79  y.setWorldCoordinates(0,0.1,0) ;
80  z.setWorldCoordinates(0,0,0.1) ;
81  traj.resize(0,2);
82 }
83 void
84 vpProjectionDisplay::init(const int select)
85 {
86  if (select & vpProjectionDisplay::internalView())
87  {
88  Icam.resize(256,256) ;
89  dIcam.init(Icam,100,100) ;
90  }
91  if (select & vpProjectionDisplay::externalView())
92  {
93  Iext.resize(256,256) ;
94  dIext.init(Iext,400,100) ;
95  }
96 
97  init() ;
98 }
99 
100 
101 void
103 {
104 
105 }
106 
107 void
109  const vpHomogeneousMatrix &cextMo,
110  const vpHomogeneousMatrix &cMo,
111  const vpCameraParameters &cam,
112  const vpColor &color,
113  const bool &displayTraj,
114  const unsigned int thickness)
115 {
116 
117  for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin() ; it != listFp.end(); ++it )
118  {
119  vpForwardProjection *fp = *it ;
120  fp->display(I,cextMo, cam, color, thickness) ;
121  }
122 
123  if(displayTraj) // display past camera positions
124  for(unsigned int i=0;i<traj.getRows();++i)
125  vpDisplay::displayCircle(I,(int)traj[i][0],(int)traj[i][1],2,vpColor::green,true);
126 
127  displayCamera(I, cextMo, cMo, cam, thickness);
128 
129  if(displayTraj) // store current camera position
130  {
131  const unsigned int n = traj.getRows();
132  traj.resize(n+1, 2, false);
133  vpMeterPixelConversion::convertPoint(cam,o.p[0],o.p[1],traj[n][1],traj[n][0]);
134  }
135 }
136 
137 
138 void
140  const vpHomogeneousMatrix &cextMo,
141  const vpHomogeneousMatrix &cMo,
142  const vpCameraParameters &cam,
143  const unsigned int thickness)
144 {
145  vpHomogeneousMatrix c1Mc ;
146  c1Mc = cextMo*cMo.inverse() ;
147 
148  o.track(c1Mc) ;
149 
150  if(o.get_Z() < 0) // do not print camera if behind the external camera
151  return;
152 
153  x.track(c1Mc) ;
154  y.track(c1Mc) ;
155  z.track(c1Mc) ;
156 
157  vpImagePoint ipo;
158  vpImagePoint ipx;
159 
160  vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
161 
162  vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
163  vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;
164 
165  vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
166  vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;
167 
168  vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
169  vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
170 }
171 
172 #elif !defined(VISP_BUILD_SHARED_LIBS)
173 // Work arround to avoid warning: libvisp_core.a(vpProjectionDisplay.cpp.o) has no symbols
174 void dummy_vpProjectionDisplay() {};
175 #endif
virtual void displayCircle(const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)=0
virtual void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1)=0
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)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
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)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
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:166
static const vpColor red
Definition: vpColor.h:163
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)
set the size of the image without initializing it.
Definition: vpImage.h:616
void insert(vpForwardProjection &fp)
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
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 init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
Definition: vpDisplayX.cpp:190
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:73
static const vpColor blue
Definition: vpColor.h:169