ViSP  2.10.0
vpFeatureBuilderPointPolar.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureBuilderPointPolar.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  * Conversion between tracker and visual feature point with
36  * polar coordinates.
37  *
38  * Authors:
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
50 #include <visp/vpFeatureBuilder.h>
51 #include <visp/vpFeatureException.h>
52 #include <visp/vpException.h>
53 
106  const vpCameraParameters &cam,
107  const vpDot &dot)
108 {
109  try {
110  double x=0, y=0;
111 
112  vpImagePoint cog;
113  cog = dot.getCog();
114 
115  vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
116 
117  double rho = sqrt(x*x + y*y);
118  double theta = atan2(y, x);
119 
120  s.set_rho(rho) ;
121  s.set_theta(theta) ;
122  }
123  catch(...) {
124  vpERROR_TRACE("Error caught") ;
125  throw ;
126  }
127 }
128 
129 
182  const vpCameraParameters &cam,
183  const vpDot2 &dot)
184 {
185  try {
186  double x=0, y=0;
187 
188  vpImagePoint cog;
189  cog = dot.getCog();
190 
191  vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
192 
193  double rho = sqrt(x*x + y*y);
194  double theta = atan2(y, x);
195 
196  s.set_rho(rho) ;
197  s.set_theta(theta) ;
198  }
199  catch(...) {
200  vpERROR_TRACE("Error caught") ;
201  throw ;
202  }
203 }
204 
205 
257  const vpCameraParameters &cam,
258  const vpImagePoint &iP)
259 {
260  try {
261  double x=0, y=0;
262 
264 
265  double rho = sqrt(x*x + y*y);
266  double theta = atan2(y, x);
267 
268  s.set_rho(rho) ;
269  s.set_theta(theta) ;
270  }
271  catch(...) {
272  vpERROR_TRACE("Error caught") ;
273  throw ;
274  }
275 }
276 
277 
296 void
298 {
299  try {
300 
301  double x = p.get_x();
302  double y = p.get_y();
303 
304  double rho = sqrt(x*x + y*y);
305  double theta = atan2(y, x);
306 
307  s.set_rho(rho) ;
308  s.set_theta(theta) ;
309 
310  s.set_Z( p.get_Z() ) ;
311 
312 
313  if (s.get_Z() < 0) {
314  vpERROR_TRACE("Point is behind the camera ") ;
315  std::cout <<"Z = " << s.get_Z() << std::endl ;
316 
318  "Point is behind the camera ")) ;
319  }
320 
321  if (fabs(s.get_Z()) < 1e-6) {
322  vpERROR_TRACE("Point Z coordinates is null ") ;
323  std::cout <<"Z = " << s.get_Z() << std::endl ;
324 
326  "Point Z coordinates is null")) ;
327  }
328 
329  }
330  catch(...) {
331  vpERROR_TRACE("Error caught") ;
332  throw ;
333  }
334 }
335 
369 void
371  const vpCameraParameters &goodCam,
372  const vpCameraParameters &wrongCam,
373  const vpPoint &p)
374 {
375  try {
376  double x = p.get_x();
377  double y = p.get_y();
378 
379  s.set_Z( p.get_Z() );
380 
381  double u=0, v=0;
382  vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
383  vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
384 
385  double rho = sqrt(x*x + y*y);
386  double theta = atan2(y, x);
387 
388  s.set_rho(rho) ;
389  s.set_theta(theta) ;
390  }
391  catch(...) {
392  vpERROR_TRACE("Error caught") ;
393  throw ;
394  }
395 }
396 
397 
398 /*
399  * Local variables:
400  * c-basic-offset: 2
401  * End:
402  */
#define vpERROR_TRACE
Definition: vpDebug.h:395
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 ...
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
void set_Z(const double Z)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:127
Class that defines 2D image point visual feature with polar coordinates described in ...
Class that defines what is a point.
Definition: vpPoint.h:65
vpImagePoint getCog() const
Definition: vpDot2.h:163
vpImagePoint getCog() const
Definition: vpDot.h:228
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.h:122
void set_theta(const double theta)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage...
Definition: vpDot.h:119
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_rho(const double rho)