Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureThetaU.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 * ThetaU visual feature.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpMath.h>
37#include <visp3/visual_features/vpBasicFeature.h>
38#include <visp3/visual_features/vpFeatureThetaU.h>
39
40// Exception
41#include <visp3/core/vpException.h>
42#include <visp3/visual_features/vpFeatureException.h>
43
44// Debug trace
45#include <visp3/core/vpDebug.h>
46
51/*
52
53attributes and members directly related to the vpBasicFeature needs
54other functionalities are useful but not mandatory
55
56*/
57
64{
65 // feature dimension
66 dim_s = 3;
67 nbParameters = 3;
68
69 // memory allocation
70 s.resize(dim_s);
71 if (flags == NULL)
72 flags = new bool[nbParameters];
73 for (unsigned int i = 0; i < nbParameters; i++)
74 flags[i] = false;
75}
76
85{
86 // vpTRACE("0x%x", this);
87 init();
88}
89
98{
99 // vpTRACE("0x%x", this);
100 init();
101
102 // kind of rotation representation
103 rotation = r;
104}
105
131
156
184
202{
203 s[0] = tu[0];
204 s[1] = tu[1];
205 s[2] = tu[2];
206 for (unsigned int i = 0; i < nbParameters; i++)
207 flags[i] = true;
208}
209
226{
227 vpThetaUVector tu(R);
228 buildFrom(tu);
229}
230
250{
252 M.extract(R);
253 vpThetaUVector tu(R);
254 buildFrom(tu);
255}
256
266
276{
277 s[0] = tu_x;
278 flags[0] = true;
279}
289{
290 s[1] = tu_y;
291 flags[1] = true;
292}
302{
303 s[2] = tu_z;
304 flags[2] = true;
305}
306
319
326double vpFeatureThetaU::get_TUx() const { return s[0]; }
327
333double vpFeatureThetaU::get_TUy() const { return s[1]; }
334
340double vpFeatureThetaU::get_TUz() const { return s[2]; }
341
405{
406
407 vpMatrix L;
408 L.resize(0, 6);
409
411 for (unsigned int i = 0; i < nbParameters; i++) {
412 if (flags[i] == false) {
413 switch (i) {
414 case 0:
415 vpTRACE("Warning !!! The interaction matrix is computed but Tu_x "
416 "was not set yet");
417 break;
418 case 1:
419 vpTRACE("Warning !!! The interaction matrix is computed but Tu_y "
420 "was not set yet");
421 break;
422 case 2:
423 vpTRACE("Warning !!! The interaction matrix is computed but Tu_z "
424 "was not set yet");
425 break;
426 default:
427 vpTRACE("Problem during the reading of the variable flags");
428 }
429 }
430 }
431 resetFlags();
432 }
433
434 // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
435 vpColVector u(3);
436 for (unsigned int i = 0; i < 3; i++) {
437 u[i] = s[i] / 2.0;
438 }
439
440 vpMatrix Lw(3, 3);
441 Lw = vpColVector::skew(u); /* [theta/2 u]_x */
442
443 vpMatrix U2(3, 3);
444 U2.eye();
445
446 double theta = sqrt(s.sumSquare());
447 if (theta >= 1e-6) {
448 for (unsigned int i = 0; i < 3; i++)
449 u[i] = s[i] / theta;
450
451 vpMatrix skew_u;
452 skew_u = vpColVector::skew(u);
453 U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u;
454 }
455
456 if (rotation == cdRc) {
457 Lw += U2;
458 } else {
459 Lw -= U2;
460 }
461
462 // This version is a simplification
463 if (vpFeatureThetaU::selectTUx() & select) {
464 vpMatrix Lx(1, 6);
465
466 Lx[0][0] = 0;
467 Lx[0][1] = 0;
468 Lx[0][2] = 0;
469 for (int i = 0; i < 3; i++)
470 Lx[0][i + 3] = Lw[0][i];
471
472 L = vpMatrix::stack(L, Lx);
473 }
474
475 if (vpFeatureThetaU::selectTUy() & select) {
476 vpMatrix Ly(1, 6);
477
478 Ly[0][0] = 0;
479 Ly[0][1] = 0;
480 Ly[0][2] = 0;
481 for (int i = 0; i < 3; i++)
482 Ly[0][i + 3] = Lw[1][i];
483
484 L = vpMatrix::stack(L, Ly);
485 }
486
487 if (vpFeatureThetaU::selectTUz() & select) {
488 vpMatrix Lz(1, 6);
489
490 Lz[0][0] = 0;
491 Lz[0][1] = 0;
492 Lz[0][2] = 0;
493 for (int i = 0; i < 3; i++)
494 Lz[0][i + 3] = Lw[2][i];
495
496 L = vpMatrix::stack(L, Lz);
497 }
498
499 return L;
500}
501
563vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, unsigned int select)
564{
565
566 if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
567 vpERROR_TRACE("s* should be zero ! ");
569 }
570
571 vpColVector e(0);
572
573 if (vpFeatureThetaU::selectTUx() & select) {
574 vpColVector ex(1);
575 ex[0] = s[0];
576 e = vpColVector::stack(e, ex);
577 }
578
579 if (vpFeatureThetaU::selectTUy() & select) {
580 vpColVector ey(1);
581 ey[0] = s[1];
582 e = vpColVector::stack(e, ey);
583 }
584
585 if (vpFeatureThetaU::selectTUz() & select) {
586 vpColVector ez(1);
587 ez[0] = s[2];
588 e = vpColVector::stack(e, ez);
589 }
590 return e;
591}
592
619void vpFeatureThetaU::print(unsigned int select) const
620{
621 std::cout << "ThetaU:";
622 if (vpFeatureThetaU::selectTUx() & select) {
623 std::cout << " tux=" << s[0];
624 }
625 if (vpFeatureThetaU::selectTUy() & select) {
626 std::cout << " tuy=" << s[1];
627 }
628 if (vpFeatureThetaU::selectTUz() & select) {
629 std::cout << " tuz=" << s[2];
630 }
631 std::cout << std::endl;
632}
633
646{
647 vpFeatureThetaU *feature;
648 if (rotation == cdRc)
650 else // if (rotation == cRcd
652
653 return feature;
654}
655
662 const vpColor & /* color */, unsigned int /* thickness */) const
663{
664 static int firsttime = 0;
665
666 if (firsttime == 0) {
667 firsttime = 1;
668 vpERROR_TRACE("not implemented");
669 // Do not throw and error since it is not subject
670 // to produce a failure
671 }
672}
678void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
679 const vpColor & /* color */, unsigned int /* thickness */) const
680{
681 static int firsttime = 0;
682
683 if (firsttime == 0) {
684 firsttime = 1;
685 vpERROR_TRACE("not implemented");
686 // Do not throw and error since it is not subject
687 // to produce a failure
688 }
689}
690
715unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
740unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
765unsigned 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.
double sumSquare() const
void stack(double d)
static vpMatrix skew(const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
vpMatrix interaction(unsigned int select=FEATURE_ALL)
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
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double sinc(double x)
Definition vpMath.cpp:264
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void eye()
Definition vpMatrix.cpp:446
void stack(const vpMatrix &A)
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:411
#define vpERROR_TRACE
Definition vpDebug.h:388