Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureBuilderPointPolar.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  * Conversion between tracker and visual feature point with
32  * polar coordinates.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
46 #include <visp3/visual_features/vpFeatureBuilder.h>
47 #include <visp3/visual_features/vpFeatureException.h>
48 #include <visp3/core/vpException.h>
49 
50 #ifdef VISP_HAVE_MODULE_BLOB
51 
103  const vpCameraParameters &cam,
104  const vpDot &dot)
105 {
106  try {
107  double x=0, y=0;
108 
109  vpImagePoint cog;
110  cog = dot.getCog();
111 
112  vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
113 
114  double rho = sqrt(x*x + y*y);
115  double theta = atan2(y, x);
116 
117  s.set_rho(rho) ;
118  s.set_theta(theta) ;
119  }
120  catch(...) {
121  vpERROR_TRACE("Error caught") ;
122  throw ;
123  }
124 }
125 
126 
179  const vpCameraParameters &cam,
180  const vpDot2 &dot)
181 {
182  try {
183  double x=0, y=0;
184 
185  vpImagePoint cog;
186  cog = dot.getCog();
187 
188  vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
189 
190  double rho = sqrt(x*x + y*y);
191  double theta = atan2(y, x);
192 
193  s.set_rho(rho) ;
194  s.set_theta(theta) ;
195  }
196  catch(...) {
197  vpERROR_TRACE("Error caught") ;
198  throw ;
199  }
200 }
201 #endif //#ifdef VISP_HAVE_MODULE_BLOB
202 
203 
255  const vpCameraParameters &cam,
256  const vpImagePoint &iP)
257 {
258  try {
259  double x=0, y=0;
260 
262 
263  double rho = sqrt(x*x + y*y);
264  double theta = atan2(y, x);
265 
266  s.set_rho(rho) ;
267  s.set_theta(theta) ;
268  }
269  catch(...) {
270  vpERROR_TRACE("Error caught") ;
271  throw ;
272  }
273 }
274 
275 
294 void
296 {
297  try {
298 
299  double x = p.get_x();
300  double y = p.get_y();
301 
302  double rho = sqrt(x*x + y*y);
303  double theta = atan2(y, x);
304 
305  s.set_rho(rho) ;
306  s.set_theta(theta) ;
307 
308  s.set_Z( p.get_Z() ) ;
309 
310 
311  if (s.get_Z() < 0) {
312  vpERROR_TRACE("Point is behind the camera ") ;
313  std::cout <<"Z = " << s.get_Z() << std::endl ;
314 
316  "Point is behind the camera ")) ;
317  }
318 
319  if (fabs(s.get_Z()) < 1e-6) {
320  vpERROR_TRACE("Point Z coordinates is null ") ;
321  std::cout <<"Z = " << s.get_Z() << std::endl ;
322 
324  "Point Z coordinates is null")) ;
325  }
326 
327  }
328  catch(...) {
329  vpERROR_TRACE("Error caught") ;
330  throw ;
331  }
332 }
333 
367 void
369  const vpCameraParameters &goodCam,
370  const vpCameraParameters &wrongCam,
371  const vpPoint &p)
372 {
373  try {
374  double x = p.get_x();
375  double y = p.get_y();
376 
377  s.set_Z( p.get_Z() );
378 
379  double u=0, v=0;
380  vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
381  vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
382 
383  double rho = sqrt(x*x + y*y);
384  double theta = atan2(y, x);
385 
386  s.set_rho(rho) ;
387  s.set_theta(theta) ;
388  }
389  catch(...) {
390  vpERROR_TRACE("Error caught") ;
391  throw ;
392  }
393 }
394 
395 
396 /*
397  * Local variables:
398  * c-basic-offset: 2
399  * End:
400  */
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
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.cpp:458
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:125
Class that defines 2D image point visual feature with polar coordinates described in ...
Class that defines what is a point.
Definition: vpPoint.h:59
vpImagePoint getCog() const
Definition: vpDot2.h:161
vpImagePoint getCog() const
Definition: vpDot.h:225
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.cpp:456
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
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:116
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 create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_rho(const double rho)