Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 
188 vpMatrix vpFeatureSegment::interaction(const unsigned int select)
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, const 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(const 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(const double x1, const double y1, const double Z1, const double x2, const double y2,
567  const double Z2)
568 {
569  double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
570  double x_c = (x1 + x2) / 2.;
571  double y_c = (y1 + y2) / 2.;
572  double alpha = atan2(y1 - y2, x1 - x2);
573 
574  if (normalized_) {
575  setXc(x_c / l);
576  setYc(y_c / l);
577  setL(1 / l);
578  setAlpha(alpha);
579 
580  setZ1(Z1);
581  setZ2(Z2);
582  } else {
583  setXc(x_c);
584  setYc(y_c);
585  setL(l);
586  setAlpha(alpha);
587 
588  setZ1(Z1);
589  setZ2(Z2);
590  }
591 }
592 
616 unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
617 
641 unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
642 
666 unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
667 
691 unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
static unsigned int selectAlpha()
void setAlpha(const double val)
static unsigned int selectYc()
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4551
unsigned int dim_s
Dimension of the visual feature.
void setL(const double val)
void setZ1(const double val)
class that defines what is a visual feature
static const vpColor cyan
Definition: vpColor.h:189
void setZ2(const double val)
static unsigned int selectL()
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
#define vpTRACE
Definition: vpDebug.h:416
Generic class defining intrinsic camera parameters.
vpFeatureSegment(bool normalized=false)
static const unsigned int FEATURE_LINE[32]
void setYc(const double val)
void buildFrom(const double x1, const double y1, const double Z1, const double x2, const double y2, const double Z2)
static void displayCircle(const vpImage< unsigned char > &I, const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)
vpBasicFeatureDeallocatorType deallocate
static double deg(double rad)
Definition: vpMath.h:101
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static const vpColor yellow
Definition: vpColor.h:188
vpFeatureSegment * duplicate() const
Feature duplication.
static unsigned int selectXc()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
void setXc(const double val)
void print(const unsigned int select=FEATURE_ALL) const