Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeaturePointPolar.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * 2D point visual feature.
33 *
34*****************************************************************************/
35
41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeaturePointPolar.h>
43
44// Exception
45#include <visp3/core/vpException.h>
46#include <visp3/visual_features/vpFeatureException.h>
47
48// Debug trace
49#include <visp3/core/vpDebug.h>
50
51// math
52#include <visp3/core/vpMath.h>
53
54#include <visp3/core/vpFeatureDisplay.h>
55
56/*
57
58 attributes and members directly related to the vpBasicFeature needs
59 other functionalities ar useful but not mandatory
60
61*/
62
73{
74 // feature dimension
75 dim_s = 2;
76 nbParameters = 3;
77
78 // memory allocation
79 s.resize(dim_s);
80 if (flags == NULL)
81 flags = new bool[nbParameters];
82 for (unsigned int i = 0; i < nbParameters; i++)
83 flags[i] = false;
84
85 // default value Z (1 meters)
86 Z = 1;
87}
88
100
107{
108 s[0] = rho;
109 flags[0] = true;
110}
117{
118 s[1] = theta;
119 flags[1] = true;
120}
121
127{
128 this->Z = Z_;
129 flags[2] = true;
130}
131
142void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, double Z_)
143{
144 set_rho(rho);
145 set_theta(theta);
146 set_Z(Z_);
147
148 for (unsigned int i = 0; i < nbParameters; i++)
149 flags[i] = true;
150}
151
157double vpFeaturePointPolar::get_rho() const { return s[0]; }
158
164double vpFeaturePointPolar::get_theta() const { return s[1]; }
169double vpFeaturePointPolar::get_Z() const { return this->Z; }
170
246{
247 vpMatrix L;
248
249 L.resize(0, 6);
250
252 for (unsigned int i = 0; i < nbParameters; i++) {
253 if (flags[i] == false) {
254 switch (i) {
255 case 0:
256 vpTRACE("Warning !!! The interaction matrix is computed but rho "
257 "was not set yet");
258 break;
259 case 1:
260 vpTRACE("Warning !!! The interaction matrix is computed but theta "
261 "was not set yet");
262 break;
263 case 2:
264 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
265 "not set yet");
266 break;
267 default:
268 vpTRACE("Problem during the reading of the variable flags");
269 }
270 }
271 }
272 resetFlags();
273 }
274
275 double rho = get_rho();
276 double theta = get_theta();
277 double Z_ = get_Z();
278
279 double c_ = cos(theta);
280 double s_ = sin(theta);
281
282 double rho2 = rho * rho;
283
284 if (fabs(rho) < 1e-6) {
285 vpERROR_TRACE("rho polar coordinate of the point is null");
286 std::cout << "rho = " << rho << std::endl;
287
288 throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
289 }
290
291 if (Z_ < 0) {
292 vpERROR_TRACE("Point is behind the camera ");
293 std::cout << "Z = " << Z_ << std::endl;
294
295 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
296 }
297
298 if (fabs(Z_) < 1e-6) {
299 vpERROR_TRACE("Point Z coordinates is null ");
300 std::cout << "Z = " << Z_ << std::endl;
301
302 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
303 }
304
305 if (vpFeaturePointPolar::selectRho() & select) {
306 vpMatrix Lrho(1, 6);
307 Lrho = 0;
308
309 Lrho[0][0] = -c_ / Z_;
310 Lrho[0][1] = -s_ / Z_;
311 Lrho[0][2] = rho / Z_;
312 Lrho[0][3] = (1 + rho2) * s_;
313 Lrho[0][4] = -(1 + rho2) * c_;
314 Lrho[0][5] = 0;
315
316 // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
317 // std::cout << "Lrho: " << Lrho << std::endl;
318
319 L = vpMatrix::stack(L, Lrho);
320 }
321
322 if (vpFeaturePointPolar::selectTheta() & select) {
323 vpMatrix Ltheta(1, 6);
324 Ltheta = 0;
325
326 Ltheta[0][0] = s_ / (rho * Z_);
327 Ltheta[0][1] = -c_ / (rho * Z_);
328 Ltheta[0][2] = 0;
329 Ltheta[0][3] = c_ / rho;
330 Ltheta[0][4] = s_ / rho;
331 Ltheta[0][5] = -1;
332
333 // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
334 // std::cout << "Ltheta: " << Ltheta << std::endl;
335 L = vpMatrix::stack(L, Ltheta);
336 }
337 return L;
338}
339
382vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, unsigned int select)
383{
384 vpColVector e(0);
385
386 try {
387 if (vpFeaturePointPolar::selectRho() & select) {
388 vpColVector erho(1);
389 erho[0] = s[0] - s_star[0];
390
391 e = vpColVector::stack(e, erho);
392 }
393
394 if (vpFeaturePointPolar::selectTheta() & select) {
395
396 // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
397 // s_star[1]);
398 double err = s[1] - s_star[1];
399
400 // printf("Error: %f ", err );
401 while (err < -M_PI)
402 err += 2 * M_PI;
403 while (err > M_PI)
404 err -= 2 * M_PI;
405 // printf(" modif %f \n", err );
406
407 vpColVector etheta(1);
408 etheta[0] = err;
409 e = vpColVector::stack(e, etheta);
410 }
411 } catch (...) {
412 throw;
413 }
414
415 return e;
416}
417
439void vpFeaturePointPolar::print(unsigned int select) const
440{
441
442 std::cout << "Point: Z=" << get_Z();
443 if (vpFeaturePointPolar::selectRho() & select)
444 std::cout << " rho=" << get_rho();
446 std::cout << " theta=" << get_theta();
447 std::cout << std::endl;
448}
449
467void vpFeaturePointPolar::buildFrom(double rho, double theta, double Z_)
468{
469
470 s[0] = rho;
471 s[1] = theta;
472
473 this->Z = Z_;
474
475 if (Z < 0) {
476 vpERROR_TRACE("Point is behind the camera ");
477 std::cout << "Z = " << Z << std::endl;
478
479 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
480 }
481
482 if (fabs(Z) < 1e-6) {
483 vpERROR_TRACE("Point Z coordinates is null ");
484 std::cout << "Z = " << Z << std::endl;
485
486 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
487 }
488
489 for (unsigned int i = 0; i < nbParameters; i++)
490 flags[i] = true;
491}
492
504 unsigned int thickness) const
505{
506 try {
507 double rho, theta;
508 rho = get_rho();
509 theta = get_theta();
510
511 double x, y;
512 x = rho * cos(theta);
513 y = rho * sin(theta);
514
515 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
516 } catch (...) {
517 vpERROR_TRACE("Error caught");
518 throw;
519 }
520}
521
533 unsigned int thickness) const
534{
535 try {
536 double rho, theta;
537 rho = get_rho();
538 theta = get_theta();
539
540 double x, y;
541 x = rho * cos(theta);
542 y = rho * sin(theta);
543
544 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
545
546 } catch (...) {
547 vpERROR_TRACE("Error caught");
548 throw;
549 }
550}
551
564{
566 return feature;
567}
568
592unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
593
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
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 Corke09a.
void buildFrom(double rho, double theta, double Z)
static unsigned int selectTheta()
void set_theta(double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpMatrix interaction(unsigned int select=FEATURE_ALL)
static unsigned int selectRho()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_rhoThetaZ(double rho, double theta, double Z)
vpFeaturePointPolar * duplicate() const
void print(unsigned int select=FEATURE_ALL) const
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388