Visual Servoing Platform  version 3.3.1 under development (2020-10-22)
vpFeatureBuilderPointPolar.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Conversion between tracker and visual feature point with
33  * polar coordinates.
34  *
35  * Authors:
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
46 #include <visp3/core/vpException.h>
47 #include <visp3/visual_features/vpFeatureBuilder.h>
48 #include <visp3/visual_features/vpFeatureException.h>
49 
50 #ifdef VISP_HAVE_MODULE_BLOB
51 
103 {
104  try {
105  double x = 0, y = 0;
106 
107  vpImagePoint cog;
108  cog = dot.getCog();
109 
110  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
111 
112  double rho = sqrt(x * x + y * y);
113  double theta = atan2(y, x);
114 
115  s.set_rho(rho);
116  s.set_theta(theta);
117  } catch (...) {
118  vpERROR_TRACE("Error caught");
119  throw;
120  }
121 }
122 
175 {
176  try {
177  double x = 0, y = 0;
178 
179  vpImagePoint cog;
180  cog = dot.getCog();
181 
182  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
183 
184  double rho = sqrt(x * x + y * y);
185  double theta = atan2(y, x);
186 
187  s.set_rho(rho);
188  s.set_theta(theta);
189  } catch (...) {
190  vpERROR_TRACE("Error caught");
191  throw;
192  }
193 }
194 #endif //#ifdef VISP_HAVE_MODULE_BLOB
195 
246 {
247  try {
248  double x = 0, y = 0;
249 
251 
252  double rho = sqrt(x * x + y * y);
253  double theta = atan2(y, x);
254 
255  s.set_rho(rho);
256  s.set_theta(theta);
257  } catch (...) {
258  vpERROR_TRACE("Error caught");
259  throw;
260  }
261 }
262 
282 {
283  try {
284 
285  double x = p.get_x();
286  double y = p.get_y();
287 
288  double rho = sqrt(x * x + y * y);
289  double theta = atan2(y, x);
290 
291  s.set_rho(rho);
292  s.set_theta(theta);
293 
294  s.set_Z(p.get_Z());
295 
296  if (s.get_Z() < 0) {
297  vpERROR_TRACE("Point is behind the camera ");
298  std::cout << "Z = " << s.get_Z() << std::endl;
299 
300  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
301  }
302 
303  if (fabs(s.get_Z()) < 1e-6) {
304  vpERROR_TRACE("Point Z coordinates is null ");
305  std::cout << "Z = " << s.get_Z() << std::endl;
306 
307  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
308  }
309 
310  } catch (...) {
311  vpERROR_TRACE("Error caught");
312  throw;
313  }
314 }
315 
350  const vpCameraParameters &wrongCam, const vpPoint &p)
351 {
352  try {
353  double x = p.get_x();
354  double y = p.get_y();
355 
356  s.set_Z(p.get_Z());
357 
358  double u = 0, v = 0;
359  vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
360  vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
361 
362  double rho = sqrt(x * x + y * y);
363  double theta = atan2(y, x);
364 
365  s.set_rho(rho);
366  s.set_theta(theta);
367  } catch (...) {
368  vpERROR_TRACE("Error caught");
369  throw;
370  }
371 }
372 
373 /*
374  * Local variables:
375  * c-basic-offset: 2
376  * End:
377  */
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:126
Class that defines 2D image point visual feature with polar coordinates described in ...
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpImagePoint getCog() const
Definition: vpDot.h:215
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_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage...
Definition: vpDot.h:115
vpImagePoint getCog() const
Definition: vpDot2.h:161
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_theta(double theta)
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:442