Visual Servoing Platform  version 3.6.1 under development (2024-04-16)
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 
53 attributes and members directly related to the vpBasicFeature needs
54 other 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 == nullptr)
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 
126 {
127  init();
128 
129  buildFrom(tu);
130 }
131 
150 {
151  init();
152 
153  vpThetaUVector tu(R);
154  buildFrom(tu);
155 }
156 
177 {
178  init();
180  M.extract(R);
181  vpThetaUVector tu(R);
182  buildFrom(tu);
183 }
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 
275 void vpFeatureThetaU::set_TUx(double tu_x)
276 {
277  s[0] = tu_x;
278  flags[0] = true;
279 }
288 void vpFeatureThetaU::set_TUy(double tu_y)
289 {
290  s[1] = tu_y;
291  flags[1] = true;
292 }
301 void vpFeatureThetaU::set_TUz(double tu_z)
302 {
303  s[2] = tu_z;
304  flags[2] = true;
305 }
306 
316 {
317  return rotation;
318 }
319 
326 double vpFeatureThetaU::get_TUx() const { return s[0]; }
327 
333 double vpFeatureThetaU::get_TUy() const { return s[1]; }
334 
340 double 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 
563 vpColVector 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 ! ");
568  throw(vpFeatureException(vpFeatureException::badInitializationError, "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 
619 void 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)
649  feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc);
650  else // if (rotation == cRcd
651  feature = new vpFeatureThetaU(vpFeatureThetaU::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 }
678 void 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 
715 unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
740 unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
765 unsigned 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:163
double sumSquare() const
void stack(double d)
static vpMatrix skew(const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
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...
vpFeatureThetaURotationRepresentationType
vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const
static unsigned int selectTUy()
vpFeatureThetaU * duplicate() const vp_override
Feature duplication.
void init() vp_override
void set_TUx(double tu_x)
double get_TUx() const
void set_TUz(double tu_z)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
void buildFrom(vpThetaUVector &tu)
double get_TUy() const
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
double get_TUz() const
static unsigned int selectTUx()
void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r)
void print(unsigned int select=FEATURE_ALL) const vp_override
static unsigned int selectTUz()
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:269
static double sqr(double x)
Definition: vpMath.h:201
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:448
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5669
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:405
#define vpERROR_TRACE
Definition: vpDebug.h:382