Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureDisplay.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpFeatureDisplay.h>
40 
41 // Meter/pixel conversion
42 #include <visp3/core/vpMeterPixelConversion.h>
43 
44 // display
45 #include <visp3/core/vpDisplay.h>
46 
47 // Debug trace
48 #include <visp3/core/vpDebug.h>
49 
50 // math
51 #include <visp3/core/vpMath.h>
52 
53 #include <visp3/core/vpImagePoint.h>
54 
55 
56 
65 void vpFeatureDisplay::displayPoint(double x,double y,
66  const vpCameraParameters &cam,
67  const vpImage<unsigned char> &I,
68  const vpColor &color,
69  unsigned int thickness)
70 {
71  try{
72  vpImagePoint ip; // pixel coordinates in float
74 
75  vpDisplay::displayCross(I, ip, 15, color, thickness) ;
76  }
77  catch(...)
78  {
79  vpERROR_TRACE("Error caught") ;
80  throw ;
81  }
82 
83 }
91 void vpFeatureDisplay::displayLine(double rho,double theta,
92  const vpCameraParameters &cam,
93  const vpImage<unsigned char> &I,
94  const vpColor &color,
95  unsigned int thickness )
96 {
97 
98 
99  try{
100  // x cos(theta) + y sin(theta) - rho = 0
101 
102  double rhop,thetap ;
103  vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
104 
105  // u cos(thetap) + v sin(thetap) - rhop = 0
106 
107  double co = cos(thetap) ;
108  double si = sin(thetap) ;
109  double c = -rhop ;
110 
111  // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
112  // double u1,v1,u2,v2 ;
113 
114  double a = si ;
115  double b = co ;
116  vpImagePoint ip1, ip2;
117 
118  if (fabs(a) < fabs(b)) {
119  ip1.set_ij(0, (-c)/b);
120  double h = I.getHeight() - 1;
121  ip2.set_ij(h, (-c - a*h)/b);
122  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
123  }
124  else {
125  ip1.set_ij((-c)/a, 0);
126  double w = I.getWidth()-1;
127  ip2.set_ij((-c - b*w)/a, w);
128  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
129  }
130  }
131  catch(...)
132  {
133  vpERROR_TRACE("Error caught") ;
134  throw ;
135  }
136 }
145 void vpFeatureDisplay::displayCylinder(double rho1,double theta1,
146  double rho2,double theta2,
147  const vpCameraParameters &cam,
148  const vpImage<unsigned char> &I,
149  const vpColor &color,
150  unsigned int thickness)
151 {
152  try
153  {
154  displayLine(rho1, theta1, cam, I, color, thickness) ;
155  displayLine(rho2, theta2, cam, I, color, thickness) ;
156  }
157  catch(...)
158  {
159  vpERROR_TRACE("Error caught") ;
160  throw ;
161  }
162 }
175 void vpFeatureDisplay::displayEllipse(double x,double y,
176  double mu20, double mu11, double mu02,
177  const vpCameraParameters &cam,
178  const vpImage<unsigned char> &I,
179  const vpColor &color,
180  unsigned int thickness)
181 {
182  try {
183  unsigned int number_of_points = 45 ;
184  const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
185  unsigned int i = 0 ;
186 
187  double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
188  double a, b, e ;
189 
190  //if (fabs(mu11)<1e-6) e =0 ;
191  if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
192  e = 0 ;
193  a = sqrt(mu20);
194  b = sqrt(mu02);
195  }
196  else {
197  e = (mu02-mu20+s)/(2*mu11) ;
198  a = sqrt( (mu02+mu20+s)/2.0) ;
199  b = sqrt( (mu02+mu20-s)/2.0) ;
200  }
201 
202  double e1 = atan(e) ;
203 
204  double k = 0.0 ;
205 
206  double ce = cos(e1) ;
207  double se = sin(e1) ;
208 
209  double x2 = 0;
210  double y2 =0;
211  vpImagePoint ip1, ip2;
212 
213  for( i = 0; i < number_of_points+2 ; i++)
214  {
215  double x1 = a *cos(k) ; // equation of an ellipse
216  double y1 = b *sin(k) ; // equation of an ellipse
217  double x11 = x + ce *x1 - se *y1 ;
218  double y11 = y + se *x1 + ce *y1 ;
219 
220  vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
221 
222  if (i > 1) {
223  ip2.set_u( x2 );
224  ip2.set_v( y2 );
225 
226  vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
227  }
228 
229  ip2 = ip1;
230  y2 = y1 ;
231  x2 = x1 ;
232  k += incr ;
233  } // end for loop
234  }
235  catch(vpException &e)
236  {
237  throw(e);
238  }
239 }
240 
249 void vpFeatureDisplay::displayPoint(double x,double y,
250  const vpCameraParameters &cam,
251  const vpImage<vpRGBa> &I,
252  const vpColor &color,
253  unsigned int thickness)
254 {
255  try{
256  vpImagePoint ip; // pixel coordinates in float
257  vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
258 
259  vpDisplay::displayCross(I, ip, 15, color, thickness) ;
260  }
261  catch(...)
262  {
263  vpERROR_TRACE("Error caught") ;
264  throw ;
265  }
266 
267 }
268 
276 void vpFeatureDisplay::displayLine(double rho,double theta,
277  const vpCameraParameters &cam,
278  const vpImage<vpRGBa> &I,
279  const vpColor &color,
280  unsigned int thickness )
281 {
282 
283 
284  try{
285  // x cos(theta) + y sin(theta) - rho = 0
286 
287  double rhop,thetap ;
288  vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
289 
290  // u cos(thetap) + v sin(thetap) - rhop = 0
291 
292  double co = cos(thetap) ;
293  double si = sin(thetap) ;
294  double c = -rhop ;
295 
296  // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
297  // double u1,v1,u2,v2 ;
298 
299  double a = si ;
300  double b = co ;
301  vpImagePoint ip1, ip2;
302 
303  if (fabs(a) < fabs(b)) {
304  ip1.set_ij(0, (-c)/b);
305  double h = I.getHeight() - 1;
306  ip2.set_ij(h, (-c - a*h)/b);
307  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
308  }
309  else {
310  ip1.set_ij((-c)/a, 0);
311  double w = I.getWidth()-1;
312  ip2.set_ij((-c - b*w)/a, w);
313  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
314  }
315  }
316  catch(...)
317  {
318  vpERROR_TRACE("Error caught") ;
319  throw ;
320  }
321 }
330 void vpFeatureDisplay::displayCylinder(double rho1, double theta1,
331  double rho2, double theta2,
332  const vpCameraParameters &cam,
333  const vpImage<vpRGBa> &I,
334  const vpColor &color,
335  unsigned int thickness)
336 {
337  try
338  {
339  displayLine(rho1, theta1, cam, I, color, thickness) ;
340  displayLine(rho2, theta2, cam, I, color, thickness) ;
341  }
342  catch(...)
343  {
344  vpERROR_TRACE("Error caught") ;
345  throw ;
346  }
347 }
348 
361 void vpFeatureDisplay::displayEllipse(double x, double y,
362  double mu20, double mu11, double mu02,
363  const vpCameraParameters &cam,
364  const vpImage<vpRGBa> &I,
365  const vpColor &color,
366  unsigned int thickness)
367 {
368  try {
369  unsigned int number_of_points = 45 ;
370  const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
371  unsigned int i = 0 ;
372 
373  double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
374  double a, b, e ;
375 
376  //if (fabs(mu11)<1e-6) e =0 ;
377  if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
378  e = 0 ;
379  a = sqrt(mu20);
380  b = sqrt(mu02);
381  }
382  else {
383  e = (mu02-mu20+s)/(2*mu11) ;
384  a = sqrt( (mu02+mu20+s)/2.0) ;
385  b = sqrt( (mu02+mu20-s)/2.0) ;
386  }
387 
388  double e1 = atan(e) ;
389 
390  double k = 0.0 ;
391 
392  double ce = cos(e1) ;
393  double se = sin(e1) ;
394 
395  double x2 = 0;
396  double y2 =0;
397  vpImagePoint ip1, ip2;
398 
399  for( i = 0; i < number_of_points+2 ; i++)
400  {
401  double x1 = a *cos(k) ; // equation of an ellipse
402  double y1 = b *sin(k) ; // equation of an ellipse
403  double x11 = x + ce *x1 - se *y1 ;
404  double y11 = y + se *x1 + ce *y1 ;
405 
406  vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
407 
408  if (i > 1) {
409  ip2.set_u( x2 );
410  ip2.set_v( y2 );
411 
412  vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
413  }
414 
415  ip2 = ip1;
416  y2 = y1 ;
417  x2 = x1 ;
418  k += incr ;
419  } // end for loop
420  }
421  catch(vpException &e)
422  {
423  throw(e);
424  }
425 }
static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
unsigned int getWidth() const
Definition: vpImage.h:226
static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
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 ...
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
error that can be emited by ViSP classes.
Definition: vpException.h:73
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void set_u(const double u)
Definition: vpImagePoint.h:221
static double sqr(double x)
Definition: vpMath.h:110
void set_v(const double v)
Definition: vpImagePoint.h:232
Generic class defining intrinsic camera parameters.
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
Line coordinates conversion (rho,theta).
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
unsigned int getHeight() const
Definition: vpImage.h:175
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:185