Visual Servoing Platform version 3.5.0
vpFeatureThetaU.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 * ThetaU visual feature.
33 *
34 * Authors:
35 * Eric Marchand
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
40#include <visp3/core/vpMath.h>
41#include <visp3/visual_features/vpBasicFeature.h>
42#include <visp3/visual_features/vpFeatureThetaU.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
55/*
56
57attributes and members directly related to the vpBasicFeature needs
58other functionalities are useful but not mandatory
59
60*/
61
68{
69 // feature dimension
70 dim_s = 3;
71 nbParameters = 3;
72
73 // memory allocation
74 s.resize(dim_s);
75 if (flags == NULL)
76 flags = new bool[nbParameters];
77 for (unsigned int i = 0; i < nbParameters; i++)
78 flags[i] = false;
79}
80
89{
90 // vpTRACE("0x%x", this);
91 init();
92}
93
102{
103 // vpTRACE("0x%x", this);
104 init();
105
106 // kind of rotation representation
107 rotation = r;
108}
109
130{
131 init();
132
133 buildFrom(tu);
134}
135
154{
155 init();
156
157 vpThetaUVector tu(R);
158 buildFrom(tu);
159}
160
181{
182 init();
184 M.extract(R);
185 vpThetaUVector tu(R);
186 buildFrom(tu);
187}
188
206{
207 s[0] = tu[0];
208 s[1] = tu[1];
209 s[2] = tu[2];
210 for (unsigned int i = 0; i < nbParameters; i++)
211 flags[i] = true;
212}
213
230{
231 vpThetaUVector tu(R);
232 buildFrom(tu);
233}
234
254{
256 M.extract(R);
257 vpThetaUVector tu(R);
258 buildFrom(tu);
259}
260
270
280{
281 s[0] = tu_x;
282 flags[0] = true;
283}
293{
294 s[1] = tu_y;
295 flags[1] = true;
296}
306{
307 s[2] = tu_z;
308 flags[2] = true;
309}
310
320{
321 return rotation;
322}
323
330double vpFeatureThetaU::get_TUx() const { return s[0]; }
331
337double vpFeatureThetaU::get_TUy() const { return s[1]; }
338
344double vpFeatureThetaU::get_TUz() const { return s[2]; }
345
409{
410
411 vpMatrix L;
412 L.resize(0, 6);
413
415 for (unsigned int i = 0; i < nbParameters; i++) {
416 if (flags[i] == false) {
417 switch (i) {
418 case 0:
419 vpTRACE("Warning !!! The interaction matrix is computed but Tu_x "
420 "was not set yet");
421 break;
422 case 1:
423 vpTRACE("Warning !!! The interaction matrix is computed but Tu_y "
424 "was not set yet");
425 break;
426 case 2:
427 vpTRACE("Warning !!! The interaction matrix is computed but Tu_z "
428 "was not set yet");
429 break;
430 default:
431 vpTRACE("Problem during the reading of the variable flags");
432 }
433 }
434 }
435 resetFlags();
436 }
437
438 // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
439 vpColVector u(3);
440 for (unsigned int i = 0; i < 3; i++) {
441 u[i] = s[i] / 2.0;
442 }
443
444 vpMatrix Lw(3, 3);
445 Lw = vpColVector::skew(u); /* [theta/2 u]_x */
446
447 vpMatrix U2(3, 3);
448 U2.eye();
449
450 double theta = sqrt(s.sumSquare());
451 if (theta >= 1e-6) {
452 for (unsigned int i = 0; i < 3; i++)
453 u[i] = s[i] / theta;
454
455 vpMatrix skew_u;
456 skew_u = vpColVector::skew(u);
457 U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u;
458 }
459
460 if (rotation == cdRc) {
461 Lw += U2;
462 } else {
463 Lw -= U2;
464 }
465
466 // This version is a simplification
467 if (vpFeatureThetaU::selectTUx() & select) {
468 vpMatrix Lx(1, 6);
469
470 Lx[0][0] = 0;
471 Lx[0][1] = 0;
472 Lx[0][2] = 0;
473 for (int i = 0; i < 3; i++)
474 Lx[0][i + 3] = Lw[0][i];
475
476 L = vpMatrix::stack(L, Lx);
477 }
478
479 if (vpFeatureThetaU::selectTUy() & select) {
480 vpMatrix Ly(1, 6);
481
482 Ly[0][0] = 0;
483 Ly[0][1] = 0;
484 Ly[0][2] = 0;
485 for (int i = 0; i < 3; i++)
486 Ly[0][i + 3] = Lw[1][i];
487
488 L = vpMatrix::stack(L, Ly);
489 }
490
491 if (vpFeatureThetaU::selectTUz() & select) {
492 vpMatrix Lz(1, 6);
493
494 Lz[0][0] = 0;
495 Lz[0][1] = 0;
496 Lz[0][2] = 0;
497 for (int i = 0; i < 3; i++)
498 Lz[0][i + 3] = Lw[2][i];
499
500 L = vpMatrix::stack(L, Lz);
501 }
502
503 return L;
504}
505
567vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, unsigned int select)
568{
569
570 if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
571 vpERROR_TRACE("s* should be zero ! ");
573 }
574
575 vpColVector e(0);
576
577 if (vpFeatureThetaU::selectTUx() & select) {
578 vpColVector ex(1);
579 ex[0] = s[0];
580 e = vpColVector::stack(e, ex);
581 }
582
583 if (vpFeatureThetaU::selectTUy() & select) {
584 vpColVector ey(1);
585 ey[0] = s[1];
586 e = vpColVector::stack(e, ey);
587 }
588
589 if (vpFeatureThetaU::selectTUz() & select) {
590 vpColVector ez(1);
591 ez[0] = s[2];
592 e = vpColVector::stack(e, ez);
593 }
594 return e;
595}
596
623void vpFeatureThetaU::print(unsigned int select) const
624{
625 std::cout << "ThetaU:";
626 if (vpFeatureThetaU::selectTUx() & select) {
627 std::cout << " tux=" << s[0];
628 }
629 if (vpFeatureThetaU::selectTUy() & select) {
630 std::cout << " tuy=" << s[1];
631 }
632 if (vpFeatureThetaU::selectTUz() & select) {
633 std::cout << " tuz=" << s[2];
634 }
635 std::cout << std::endl;
636}
637
650{
651 vpFeatureThetaU *feature;
652 if (rotation == cdRc)
654 else // if (rotation == cRcd
656
657 return feature;
658}
659
666 const vpColor & /* color */, unsigned int /* thickness */) const
667{
668 static int firsttime = 0;
669
670 if (firsttime == 0) {
671 firsttime = 1;
672 vpERROR_TRACE("not implemented");
673 // Do not throw and error since it is not subject
674 // to produce a failure
675 }
676}
682void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
683 const vpColor & /* color */, unsigned int /* thickness */) const
684{
685 static int firsttime = 0;
686
687 if (firsttime == 0) {
688 firsttime = 1;
689 vpERROR_TRACE("not implemented");
690 // Do not throw and error since it is not subject
691 // to produce a failure
692 }
693}
694
719unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
744unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
769unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2]; }
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.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
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
double sumSquare() const
void stack(double d)
static vpMatrix skew(const vpColVector &v)
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
Error that can be emited by the vpBasicFeature class and its derivates.
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpFeatureThetaURotationRepresentationType
vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const
static unsigned int selectTUy()
vpFeatureThetaU * duplicate() const
Feature duplication.
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_TUx(double tu_x)
double get_TUx() const
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_TUz(double tu_z)
void buildFrom(vpThetaUVector &tu)
double get_TUy() const
double get_TUz() const
static unsigned int selectTUx()
void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r)
static unsigned int selectTUz()
void print(unsigned int select=FEATURE_ALL) const
void set_TUy(double tu_y)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sinc(double x)
Definition: vpMath.cpp:210
static double sqr(double x)
Definition: vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:449
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393