Visual Servoing Platform  version 3.6.1 under development (2024-12-10)
vpFeatureBuilderPointPolar.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/visual_features/vpFeatureBuilder.h>
44 #include <visp3/visual_features/vpFeatureException.h>
45 
46 BEGIN_VISP_NAMESPACE
47 #ifdef VISP_HAVE_MODULE_BLOB
100 {
101  try {
102  double x = 0, y = 0;
103 
104  vpImagePoint cog;
105  cog = dot.getCog();
106 
107  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
108 
109  double rho = sqrt(x * x + y * y);
110  double theta = atan2(y, x);
111 
112  s.set_rho(rho);
113  s.set_theta(theta);
114  }
115  catch (...) {
116  vpERROR_TRACE("Error caught");
117  throw;
118  }
119 }
120 
173 {
174  try {
175  double x = 0, y = 0;
176 
177  vpImagePoint cog;
178  cog = dot.getCog();
179 
180  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
181 
182  double rho = sqrt(x * x + y * y);
183  double theta = atan2(y, x);
184 
185  s.set_rho(rho);
186  s.set_theta(theta);
187  }
188  catch (...) {
189  vpERROR_TRACE("Error caught");
190  throw;
191  }
192 }
193 #endif //#ifdef VISP_HAVE_MODULE_BLOB
194 
245 {
246  try {
247  double x = 0, y = 0;
248 
250 
251  double rho = sqrt(x * x + y * y);
252  double theta = atan2(y, x);
253 
254  s.set_rho(rho);
255  s.set_theta(theta);
256  }
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  }
311  catch (...) {
312  vpERROR_TRACE("Error caught");
313  throw;
314  }
315 }
316 
351  const vpCameraParameters &wrongCam, const vpPoint &p)
352 {
353  try {
354  double x = p.get_x();
355  double y = p.get_y();
356 
357  s.set_Z(p.get_Z());
358 
359  double u = 0, v = 0;
360  vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
361  vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
362 
363  double rho = sqrt(x * x + y * y);
364  double theta = atan2(y, x);
365 
366  s.set_rho(rho);
367  s.set_theta(theta);
368  }
369  catch (...) {
370  vpERROR_TRACE("Error caught");
371  throw;
372  }
373 }
374 END_VISP_NAMESPACE
375 /*
376  * Local variables:
377  * c-basic-offset: 2
378  * End:
379  */
Generic class defining intrinsic camera parameters.
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
Definition: vpDot2.h:125
vpImagePoint getCog() const
Definition: vpDot2.h:181
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
Definition: vpDot.h:116
vpImagePoint getCog() const
Definition: vpDot.h:247
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines 2D image point visual feature with polar coordinates described in .
void set_theta(double theta)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:410