Visual Servoing Platform version 3.5.0
vpFeaturePointPolar.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 * 2D point visual feature.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
44#include <visp3/visual_features/vpBasicFeature.h>
45#include <visp3/visual_features/vpFeaturePointPolar.h>
46
47// Exception
48#include <visp3/core/vpException.h>
49#include <visp3/visual_features/vpFeatureException.h>
50
51// Debug trace
52#include <visp3/core/vpDebug.h>
53
54// math
55#include <visp3/core/vpMath.h>
56
57#include <visp3/core/vpFeatureDisplay.h>
58
59/*
60
61 attributes and members directly related to the vpBasicFeature needs
62 other functionalities ar useful but not mandatory
63
64*/
65
76{
77 // feature dimension
78 dim_s = 2;
79 nbParameters = 3;
80
81 // memory allocation
82 s.resize(dim_s);
83 if (flags == NULL)
84 flags = new bool[nbParameters];
85 for (unsigned int i = 0; i < nbParameters; i++)
86 flags[i] = false;
87
88 // default value Z (1 meters)
89 Z = 1;
90}
91
103
110{
111 s[0] = rho;
112 flags[0] = true;
113}
120{
121 s[1] = theta;
122 flags[1] = true;
123}
124
130{
131 this->Z = Z_;
132 flags[2] = true;
133}
134
145void vpFeaturePointPolar::set_rhoThetaZ(double rho, double theta, double Z_)
146{
147 set_rho(rho);
148 set_theta(theta);
149 set_Z(Z_);
150
151 for (unsigned int i = 0; i < nbParameters; i++)
152 flags[i] = true;
153}
154
160double vpFeaturePointPolar::get_rho() const { return s[0]; }
161
167double vpFeaturePointPolar::get_theta() const { return s[1]; }
172double vpFeaturePointPolar::get_Z() const { return this->Z; }
173
249{
250 vpMatrix L;
251
252 L.resize(0, 6);
253
255 for (unsigned int i = 0; i < nbParameters; i++) {
256 if (flags[i] == false) {
257 switch (i) {
258 case 0:
259 vpTRACE("Warning !!! The interaction matrix is computed but rho "
260 "was not set yet");
261 break;
262 case 1:
263 vpTRACE("Warning !!! The interaction matrix is computed but theta "
264 "was not set yet");
265 break;
266 case 2:
267 vpTRACE("Warning !!! The interaction matrix is computed but Z was "
268 "not set yet");
269 break;
270 default:
271 vpTRACE("Problem during the reading of the variable flags");
272 }
273 }
274 }
275 resetFlags();
276 }
277
278 double rho = get_rho();
279 double theta = get_theta();
280 double Z_ = get_Z();
281
282 double c_ = cos(theta);
283 double s_ = sin(theta);
284
285 double rho2 = rho * rho;
286
287 if (fabs(rho) < 1e-6) {
288 vpERROR_TRACE("rho polar coordinate of the point is null");
289 std::cout << "rho = " << rho << std::endl;
290
291 throw(vpFeatureException(vpFeatureException::badInitializationError, "rho polar coordinate of the point is null"));
292 }
293
294 if (Z_ < 0) {
295 vpERROR_TRACE("Point is behind the camera ");
296 std::cout << "Z = " << Z_ << std::endl;
297
298 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
299 }
300
301 if (fabs(Z_) < 1e-6) {
302 vpERROR_TRACE("Point Z coordinates is null ");
303 std::cout << "Z = " << Z_ << std::endl;
304
305 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
306 }
307
308 if (vpFeaturePointPolar::selectRho() & select) {
309 vpMatrix Lrho(1, 6);
310 Lrho = 0;
311
312 Lrho[0][0] = -c_ / Z_;
313 Lrho[0][1] = -s_ / Z_;
314 Lrho[0][2] = rho / Z_;
315 Lrho[0][3] = (1 + rho2) * s_;
316 Lrho[0][4] = -(1 + rho2) * c_;
317 Lrho[0][5] = 0;
318
319 // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
320 // std::cout << "Lrho: " << Lrho << std::endl;
321
322 L = vpMatrix::stack(L, Lrho);
323 }
324
325 if (vpFeaturePointPolar::selectTheta() & select) {
326 vpMatrix Ltheta(1, 6);
327 Ltheta = 0;
328
329 Ltheta[0][0] = s_ / (rho * Z_);
330 Ltheta[0][1] = -c_ / (rho * Z_);
331 Ltheta[0][2] = 0;
332 Ltheta[0][3] = c_ / rho;
333 Ltheta[0][4] = s_ / rho;
334 Ltheta[0][5] = -1;
335
336 // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
337 // std::cout << "Ltheta: " << Ltheta << std::endl;
338 L = vpMatrix::stack(L, Ltheta);
339 }
340 return L;
341}
342
385vpColVector vpFeaturePointPolar::error(const vpBasicFeature &s_star, unsigned int select)
386{
387 vpColVector e(0);
388
389 try {
390 if (vpFeaturePointPolar::selectRho() & select) {
391 vpColVector erho(1);
392 erho[0] = s[0] - s_star[0];
393
394 e = vpColVector::stack(e, erho);
395 }
396
397 if (vpFeaturePointPolar::selectTheta() & select) {
398
399 // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
400 // s_star[1]);
401 double err = s[1] - s_star[1];
402
403 // printf("Error: %f ", err );
404 while (err < -M_PI)
405 err += 2 * M_PI;
406 while (err > M_PI)
407 err -= 2 * M_PI;
408 // printf(" modif %f \n", err );
409
410 vpColVector etheta(1);
411 etheta[0] = err;
412 e = vpColVector::stack(e, etheta);
413 }
414 } catch (...) {
415 throw;
416 }
417
418 return e;
419}
420
442void vpFeaturePointPolar::print(unsigned int select) const
443{
444
445 std::cout << "Point: Z=" << get_Z();
446 if (vpFeaturePointPolar::selectRho() & select)
447 std::cout << " rho=" << get_rho();
449 std::cout << " theta=" << get_theta();
450 std::cout << std::endl;
451}
452
470void vpFeaturePointPolar::buildFrom(double rho, double theta, double Z_)
471{
472
473 s[0] = rho;
474 s[1] = theta;
475
476 this->Z = Z_;
477
478 if (Z < 0) {
479 vpERROR_TRACE("Point is behind the camera ");
480 std::cout << "Z = " << Z << std::endl;
481
482 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
483 }
484
485 if (fabs(Z) < 1e-6) {
486 vpERROR_TRACE("Point Z coordinates is null ");
487 std::cout << "Z = " << Z << std::endl;
488
489 throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
490 }
491
492 for (unsigned int i = 0; i < nbParameters; i++)
493 flags[i] = true;
494}
495
507 unsigned int thickness) const
508{
509 try {
510 double rho, theta;
511 rho = get_rho();
512 theta = get_theta();
513
514 double x, y;
515 x = rho * cos(theta);
516 y = rho * sin(theta);
517
518 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
519 } catch (...) {
520 vpERROR_TRACE("Error caught");
521 throw;
522 }
523}
524
536 unsigned int thickness) const
537{
538 try {
539 double rho, theta;
540 rho = get_rho();
541 theta = get_theta();
542
543 double x, y;
544 x = rho * cos(theta);
545 y = rho * sin(theta);
546
547 vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
548
549 } catch (...) {
550 vpERROR_TRACE("Error caught");
551 throw;
552 }
553}
554
567{
569 return feature;
570}
571
595unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
596
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.
Definition: vpColVector.h:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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 emited by the vpBasicFeature class and its derivates.
Class that defines 2D image point visual feature with polar coordinates described in .
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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393