Visual Servoing Platform  version 3.5.1 under development (2022-07-05)
vpFeatureSegment.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  * Segment visual feature.
33  *
34  * Authors:
35  * Filip Novotny
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <cmath>
41 #include <visp3/core/vpDisplay.h>
42 #include <visp3/core/vpImagePoint.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpMeterPixelConversion.h>
45 #include <visp3/visual_features/vpBasicFeature.h>
46 #include <visp3/visual_features/vpFeatureSegment.h>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 
51 // Debug trace
52 #include <visp3/core/vpDebug.h>
53 
66 {
67  // feature dimension
68  dim_s = 4;
69  nbParameters = 6;
70 
71  // memory allocation
72  s.resize(dim_s);
73  if (flags == NULL)
74  flags = new bool[nbParameters];
75  for (unsigned int i = 0; i < nbParameters; i++)
76  flags[i] = false;
77 }
78 
87  : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
88 {
89  init();
90 }
91 
189 {
190 
191  vpMatrix L;
192  L.resize(0, 6);
193 
195  for (unsigned int i = 0; i < nbParameters; i++) {
196  if (flags[i] == false) {
197  switch (i) {
198  case 0:
199  vpTRACE("Warning !!! The interaction matrix is computed but xc "
200  "was not set yet");
201  break;
202  case 1:
203  vpTRACE("Warning !!! The interaction matrix is computed but Yc "
204  "was not set yet");
205  break;
206  case 2:
207  vpTRACE("Warning !!! The interaction matrix is computed but l was "
208  "not set yet");
209  break;
210  case 3:
211  vpTRACE("Warning !!! The interaction matrix is computed but alpha "
212  "was not set yet");
213  break;
214  case 4:
215  vpTRACE("Warning !!! The interaction matrix is computed but Z1 "
216  "was not set yet");
217  break;
218  case 5:
219  vpTRACE("Warning !!! The interaction matrix is computed but Z2 "
220  "was not set yet");
221  break;
222  default:
223  vpTRACE("Problem during the reading of the variable flags");
224  }
225  }
226  }
227  }
228 
229  // This version is a simplification
230  double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_); // -l * lambda
231  double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_); // 1/Zm
232 
233  if (normalized_) {
234  // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
235  double xn = xc_;
236  double yn = yc_;
237  double ln = l_;
238  double lambda = -lambda1 * ln;
239  double Zn_inv = lambda2 * ln;
240  double lc = cos_a_ / ln;
241  double ls = sin_a_ / ln;
242  double xnalpha = xn * cos_a_ + yn * sin_a_;
243  double lnc = cos_a_ * ln;
244  double lns = sin_a_ * ln;
245 
246  if (vpFeatureSegment::selectXc() & select) {
247  vpMatrix Lxn(1, 6);
248  Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
249  Lxn[0][1] = lambda * xn * sin_a_;
250  Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
251  Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
252  Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
253  Lxn[0][5] = yn;
254  L = vpMatrix::stack(L, Lxn);
255  }
256 
257  if (vpFeatureSegment::selectYc() & select) {
258  vpMatrix Lyn(1, 6);
259  Lyn[0][0] = lambda * yn * cos_a_;
260  Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
261  Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
262  Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
263  Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
264  Lyn[0][5] = -xn;
265  L = vpMatrix::stack(L, Lyn);
266  }
267 
268  if (vpFeatureSegment::selectL() & select) {
269  vpMatrix Lln(1, 6);
270  Lln[0][0] = lambda * lnc;
271  Lln[0][1] = lambda * lns;
272  Lln[0][2] = -(Zn_inv + lambda * xnalpha);
273  Lln[0][3] = -yn - xnalpha * sin_a_;
274  Lln[0][4] = xn + xnalpha * cos_a_;
275  Lln[0][5] = 0;
276  L = vpMatrix::stack(L, Lln);
277  }
278  if (vpFeatureSegment::selectAlpha() & select) {
279  // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains
280  // 1/l
281  vpMatrix Lalpha(1, 6);
282  Lalpha[0][0] = -lambda1 * sin_a_ * l_;
283  Lalpha[0][1] = lambda1 * cos_a_ * l_;
284  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
285  Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
286  Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
287  Lalpha[0][5] = -1;
288  L = vpMatrix::stack(L, Lalpha);
289  }
290  } else {
291  if (vpFeatureSegment::selectXc() & select) {
292  vpMatrix Lxc(1, 6);
293  Lxc[0][0] = -lambda2;
294  Lxc[0][1] = 0.;
295  Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
296  Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
297  Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
298  Lxc[0][5] = yc_;
299  L = vpMatrix::stack(L, Lxc);
300  }
301 
302  if (vpFeatureSegment::selectYc() & select) {
303  vpMatrix Lyc(1, 6);
304  Lyc[0][0] = 0.;
305  Lyc[0][1] = -lambda2;
306  Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
307  Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
308  Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
309  Lyc[0][5] = -xc_;
310  L = vpMatrix::stack(L, Lyc);
311  }
312 
313  if (vpFeatureSegment::selectL() & select) {
314  vpMatrix Ll(1, 6);
315  Ll[0][0] = lambda1 * cos_a_;
316  Ll[0][1] = lambda1 * sin_a_;
317  Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
318  Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
319  Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
320  Ll[0][5] = 0;
321  L = vpMatrix::stack(L, Ll);
322  }
323  if (vpFeatureSegment::selectAlpha() & select) {
324  vpMatrix Lalpha(1, 6);
325  Lalpha[0][0] = -lambda1 * sin_a_ / l_;
326  Lalpha[0][1] = lambda1 * cos_a_ / l_;
327  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
328  Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
329  Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
330  Lalpha[0][5] = -1;
331  L = vpMatrix::stack(L, Lalpha);
332  }
333  }
334 
335  return L;
336 }
337 
363 vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, unsigned int select)
364 {
365  vpColVector e(0);
366 
367  if (vpFeatureSegment::selectXc() & select) {
368  vpColVector exc(1);
369  exc[0] = xc_ - s_star[0];
370  e = vpColVector::stack(e, exc);
371  }
372 
373  if (vpFeatureSegment::selectYc() & select) {
374  vpColVector eyc(1);
375  eyc[0] = yc_ - s_star[1];
376  e = vpColVector::stack(e, eyc);
377  }
378 
379  if (vpFeatureSegment::selectL() & select) {
380  vpColVector eL(1);
381  eL[0] = l_ - s_star[2];
382  e = vpColVector::stack(e, eL);
383  }
384 
385  if (vpFeatureSegment::selectAlpha() & select) {
386  vpColVector eAlpha(1);
387  eAlpha[0] = alpha_ - s_star[3];
388  while (eAlpha[0] < -M_PI)
389  eAlpha[0] += 2 * M_PI;
390  while (eAlpha[0] > M_PI)
391  eAlpha[0] -= 2 * M_PI;
392  e = vpColVector::stack(e, eAlpha);
393  }
394  return e;
395 }
396 
424 void vpFeatureSegment::print(unsigned int select) const
425 {
426  std::cout << "vpFeatureSegment: (";
427  if (vpFeatureSegment::selectXc() & select) {
428  if (normalized_)
429  std::cout << "xn = ";
430  else
431  std::cout << "xc = ";
432  std::cout << s[0] << "; ";
433  }
434  if (vpFeatureSegment::selectYc() & select) {
435  if (normalized_)
436  std::cout << "yn = ";
437  else
438  std::cout << "yc = ";
439  std::cout << s[1] << "; ";
440  }
441  if (vpFeatureSegment::selectL() & select) {
442  if (normalized_)
443  std::cout << "ln = ";
444  else
445  std::cout << "l = ";
446  std::cout << s[2] << "; ";
447  }
448  if (vpFeatureSegment::selectAlpha() & select) {
449  std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
450  }
451  std::cout << ")" << std::endl;
452 }
453 
465 {
466  vpFeatureSegment *feature;
467 
468  feature = new vpFeatureSegment(*this);
469  return feature;
470 }
471 
484  unsigned int thickness) const
485 {
486  double l, x, y;
487  if (normalized_) {
488  l = 1. / l_;
489  x = xc_ * l;
490  y = yc_ * l;
491  } else {
492  l = l_;
493  x = xc_;
494  y = yc_;
495  }
496 
497  double x1 = x - (l / 2.) * cos_a_;
498  double x2 = x + (l / 2.) * cos_a_;
499 
500  double y1 = y - (l / 2.) * sin_a_;
501  double y2 = y + (l / 2.) * sin_a_;
502  vpImagePoint ip1, ip2;
503 
504  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
505  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
506  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
507  vpDisplay::displayCircle(I, ip1, 5, color, true);
508  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
509 }
510 
521 void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
522  unsigned int thickness) const
523 {
524  double l, x, y;
525  if (normalized_) {
526  l = 1. / l_;
527  x = xc_ * l;
528  y = yc_ * l;
529  } else {
530  l = l_;
531  x = xc_;
532  y = yc_;
533  }
534 
535  double x1 = x - (l / 2.) * cos_a_;
536  double x2 = x + (l / 2.) * cos_a_;
537 
538  double y1 = y - (l / 2.) * sin_a_;
539  double y2 = y + (l / 2.) * sin_a_;
540  vpImagePoint ip1, ip2;
541 
542  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
543  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
544  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
545  vpDisplay::displayCircle(I, ip1, 5, vpColor::cyan, true);
546  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
547 }
548 
566 void vpFeatureSegment::buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
567 {
568  double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
569  double x_c = (x1 + x2) / 2.;
570  double y_c = (y1 + y2) / 2.;
571  double alpha = atan2(y1 - y2, x1 - x2);
572 
573  if (normalized_) {
574  setXc(x_c / l);
575  setYc(y_c / l);
576  setL(1 / l);
577  setAlpha(alpha);
578 
579  setZ1(Z1);
580  setZ2(Z2);
581  } else {
582  setXc(x_c);
583  setYc(y_c);
584  setL(l);
585  setAlpha(alpha);
586 
587  setZ1(Z1);
588  setZ2(Z2);
589  }
590 }
591 
615 unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
616 
640 unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
641 
665 unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
666 
690 unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3]; }
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:314
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor yellow
Definition: vpColor.h:225
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCircle(const vpImage< unsigned char > &I, const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void print(unsigned int select=FEATURE_ALL) const
static unsigned int selectAlpha()
void buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
static unsigned int selectXc()
void setAlpha(double val)
static unsigned int selectYc()
void setL(double val)
void setZ2(double val)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpFeatureSegment * duplicate() const
Feature duplication.
vpFeatureSegment(bool normalized=false)
void setZ1(double val)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void setXc(double val)
static unsigned int selectL()
void setYc(double val)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5871
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpTRACE
Definition: vpDebug.h:416