ViSP  2.9.0
vpFeatureDisplay.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureDisplay.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface with the image for feature display.
36  *
37  * Authors:
38  * Eric Marchand
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 #include <visp/vpFeatureDisplay.h>
44 
45 // Meter/pixel conversion
46 #include <visp/vpMeterPixelConversion.h>
47 
48 // display
49 #include <visp/vpDisplay.h>
50 
51 // Debug trace
52 #include <visp/vpDebug.h>
53 
54 // math
55 #include <visp/vpMath.h>
56 
57 #include <visp/vpImagePoint.h>
58 
59 
60 
69 void vpFeatureDisplay::displayPoint(double x,double y,
70  const vpCameraParameters &cam,
71  const vpImage<unsigned char> &I,
72  const vpColor &color,
73  unsigned int thickness)
74 {
75  try{
76  vpImagePoint ip; // pixel coordinates in float
78 
79  vpDisplay::displayCross(I, ip, 15, color, thickness) ;
80  }
81  catch(...)
82  {
83  vpERROR_TRACE("Error caught") ;
84  throw ;
85  }
86 
87 }
95 void vpFeatureDisplay::displayLine(double rho,double theta,
96  const vpCameraParameters &cam,
97  const vpImage<unsigned char> &I,
98  const vpColor &color,
99  unsigned int thickness )
100 {
101 
102 
103  try{
104  // x cos(theta) + y sin(theta) - rho = 0
105 
106  double rhop,thetap ;
107  vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
108 
109  // u cos(thetap) + v sin(thetap) - rhop = 0
110 
111  double co = cos(thetap) ;
112  double si = sin(thetap) ;
113  double c = -rhop ;
114 
115  // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
116  // double u1,v1,u2,v2 ;
117 
118  double a = si ;
119  double b = co ;
120  vpImagePoint ip1, ip2;
121 
122  if (fabs(a) < fabs(b)) {
123  ip1.set_ij(0, (-c)/b);
124  double h = I.getHeight() - 1;
125  ip2.set_ij(h, (-c - a*h)/b);
126  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
127  }
128  else {
129  ip1.set_ij((-c)/a, 0);
130  double w = I.getWidth()-1;
131  ip2.set_ij((-c - b*w)/a, w);
132  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
133  }
134  }
135  catch(...)
136  {
137  vpERROR_TRACE("Error caught") ;
138  throw ;
139  }
140 }
149 void vpFeatureDisplay::displayCylinder(double rho1,double theta1,
150  double rho2,double theta2,
151  const vpCameraParameters &cam,
152  const vpImage<unsigned char> &I,
153  const vpColor &color,
154  unsigned int thickness)
155 {
156  try
157  {
158  displayLine(rho1, theta1, cam, I, color, thickness) ;
159  displayLine(rho2, theta2, cam, I, color, thickness) ;
160  }
161  catch(...)
162  {
163  vpERROR_TRACE("Error caught") ;
164  throw ;
165  }
166 }
174 void vpFeatureDisplay::displayEllipse(double x,double y,
175  double mu20, double mu11, double mu02,
176  const vpCameraParameters &cam,
177  const vpImage<unsigned char> &I,
178  const vpColor &color,
179  unsigned int thickness)
180 {
181 
182 
183  try{
184  {
185  unsigned int number_of_points = 45 ;
186  const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
187  unsigned int i = 0 ;
188 
189  // std::cout << s.t() ;
190  double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
191  double e ;
192 
193  if (fabs(mu11)<1e-6) e =0 ;
194  else e = (mu02-mu20+s)/(2*mu11) ;
195  double a =sqrt( (mu02+mu20+s)/2.0) ;
196  double b =sqrt( (mu02+mu20-s)/2.0) ;
197 
198  // vpTRACE("%f %f %f", a,b,e) ;
199 
200  double e1 = atan(e) ;
201 
202  double k = 0.0 ;
203 
204  double ce = cos(e1) ;
205  double se = sin(e1) ;
206 
207  double x2 = 0;
208  double y2 =0;
209  vpImagePoint ip1, ip2;
210 
211  for( i = 0; i < number_of_points+2 ; i++)
212  {
213  double x1 = a *cos(k) ; // equation of an ellipse
214  double y1 = b *sin(k) ; // equation of an ellipse
215  double x11 = x + ce *x1 - se *y1 ;
216  double y11 = y + se *x1 + ce *y1 ;
217 
218  vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
219 
220  if (i > 1) {
221  ip2.set_u( x2 );
222  ip2.set_v( y2 );
223 
224  vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
225  }
226 
227  ip2 = ip1;
228  y2 = y1 ;
229  x2 = x1 ;
230  k += incr ;
231  } // end for loop
232  }
233  // vpDisplay::getClick(I) ;
234  }
235  catch(...)
236  {
237  vpERROR_TRACE("Error caught") ;
238  throw ;
239  }
240 }
241 
250 void vpFeatureDisplay::displayPoint(double x,double y,
251  const vpCameraParameters &cam,
252  const vpImage<vpRGBa> &I,
253  const vpColor &color,
254  unsigned int thickness)
255 {
256  try{
257  vpImagePoint ip; // pixel coordinates in float
258  vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
259 
260  vpDisplay::displayCross(I, ip, 15, color, thickness) ;
261  }
262  catch(...)
263  {
264  vpERROR_TRACE("Error caught") ;
265  throw ;
266  }
267 
268 }
269 
277 void vpFeatureDisplay::displayLine(double rho,double theta,
278  const vpCameraParameters &cam,
279  const vpImage<vpRGBa> &I,
280  const vpColor &color,
281  unsigned int thickness )
282 {
283 
284 
285  try{
286  // x cos(theta) + y sin(theta) - rho = 0
287 
288  double rhop,thetap ;
289  vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
290 
291  // u cos(thetap) + v sin(thetap) - rhop = 0
292 
293  double co = cos(thetap) ;
294  double si = sin(thetap) ;
295  double c = -rhop ;
296 
297  // vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
298  // double u1,v1,u2,v2 ;
299 
300  double a = si ;
301  double b = co ;
302  vpImagePoint ip1, ip2;
303 
304  if (fabs(a) < fabs(b)) {
305  ip1.set_ij(0, (-c)/b);
306  double h = I.getHeight() - 1;
307  ip2.set_ij(h, (-c - a*h)/b);
308  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
309  }
310  else {
311  ip1.set_ij((-c)/a, 0);
312  double w = I.getWidth()-1;
313  ip2.set_ij((-c - b*w)/a, w);
314  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
315  }
316  }
317  catch(...)
318  {
319  vpERROR_TRACE("Error caught") ;
320  throw ;
321  }
322 }
331 void vpFeatureDisplay::displayCylinder(double rho1, double theta1,
332  double rho2, double theta2,
333  const vpCameraParameters &cam,
334  const vpImage<vpRGBa> &I,
335  const vpColor &color,
336  unsigned int thickness)
337 {
338  try
339  {
340  displayLine(rho1, theta1, cam, I, color, thickness) ;
341  displayLine(rho2, theta2, cam, I, color, thickness) ;
342  }
343  catch(...)
344  {
345  vpERROR_TRACE("Error caught") ;
346  throw ;
347  }
348 }
349 
357 void vpFeatureDisplay::displayEllipse(double x, double y,
358  double mu20, double mu11, double mu02,
359  const vpCameraParameters &cam,
360  const vpImage<vpRGBa> &I,
361  const vpColor &color,
362  unsigned int thickness)
363 {
364 
365 
366  try{
367  {
368  unsigned int number_of_points = 45 ;
369  const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
370  unsigned int i = 0 ;
371 
372  // std::cout << s.t() ;
373  double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
374  double e ;
375 
376  if (fabs(mu11)<1e-6) e =0 ;
377  else e = (mu02-mu20+s)/(2*mu11) ;
378  double a =sqrt( (mu02+mu20+s)/2.0) ;
379  double b =sqrt( (mu02+mu20-s)/2.0) ;
380 
381  // vpTRACE("%f %f %f", a,b,e) ;
382 
383  double e1 = atan(e) ;
384 
385  double k = 0.0 ;
386 
387  double ce = cos(e1) ;
388  double se = sin(e1) ;
389  double x2 = 0;
390  double y2 =0;
391  vpImagePoint ip1, ip2;
392 
393  for( i = 0; i < number_of_points+2 ; i++)
394  {
395  double x1 = a *cos(k) ; // equation of an ellipse
396  double y1 = b *sin(k) ; // equation of an ellipse
397  double x11 = x + ce *x1 - se *y1 ;
398  double y11 = y + se *x1 + ce *y1 ;
399 
400  vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
401 
402  if (i > 1) {
403  ip2.set_u( x2 );
404  ip2.set_v( y2 );
405 
406  vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
407  }
408 
409  ip2 = ip1;
410  k += incr ;
411  } // end for loop
412  }
413  // vpDisplay::getClick(I) ;
414  }
415  catch(...)
416  {
417  vpERROR_TRACE("Error caught") ;
418  throw ;
419  }
420 }
421 
422 
423 /*
424  * Local variables:
425  * c-basic-offset: 2
426  * End:
427  */
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:159
#define vpERROR_TRACE
Definition: vpDebug.h:395
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 ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
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:216
static double sqr(double x)
Definition: vpMath.h:106
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
Definition: vpImagePoint.h:227
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 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:150
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:92
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:180