Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
vpFeatureSegment.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Segment visual feature.
32  */
33 
39 #include <cmath>
40 #include <visp3/core/vpDisplay.h>
41 #include <visp3/core/vpImagePoint.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeatureSegment.h>
46 
47 // Exception
48 #include <visp3/core/vpException.h>
49 
50 // Debug trace
51 #include <visp3/core/vpDebug.h>
52 
53 BEGIN_VISP_NAMESPACE
54 
59 {
60  // feature dimension
61  dim_s = 4;
62  nbParameters = 6;
63 
64  // memory allocation
65  s.resize(dim_s);
66  if (flags == nullptr)
67  flags = new bool[nbParameters];
68  for (unsigned int i = 0; i < nbParameters; i++)
69  flags[i] = false;
70 }
71 
80  : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
81 {
82  init();
83 }
84 
185 {
186  vpMatrix L;
187  L.resize(0, 6);
188 
190  for (unsigned int i = 0; i < nbParameters; i++) {
191  if (flags[i] == false) {
192  switch (i) {
193  case 0:
194  vpTRACE("Warning !!! The interaction matrix is computed but xc "
195  "was not set yet");
196  break;
197  case 1:
198  vpTRACE("Warning !!! The interaction matrix is computed but Yc "
199  "was not set yet");
200  break;
201  case 2:
202  vpTRACE("Warning !!! The interaction matrix is computed but l was "
203  "not set yet");
204  break;
205  case 3:
206  vpTRACE("Warning !!! The interaction matrix is computed but alpha "
207  "was not set yet");
208  break;
209  case 4:
210  vpTRACE("Warning !!! The interaction matrix is computed but Z1 "
211  "was not set yet");
212  break;
213  case 5:
214  vpTRACE("Warning !!! The interaction matrix is computed but Z2 "
215  "was not set yet");
216  break;
217  default:
218  vpTRACE("Problem during the reading of the variable flags");
219  }
220  }
221  }
222  }
223 
224  // This version is a simplification
225  double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_); // -l * lambda
226  double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_); // 1/Zm
227 
228  if (normalized_) {
229  // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
230  double xn = xc_;
231  double yn = yc_;
232  double ln = l_;
233  double lambda = -lambda1 * ln;
234  double Zn_inv = lambda2 * ln;
235  double lc = cos_a_ / ln;
236  double ls = sin_a_ / ln;
237  double xnalpha = xn * cos_a_ + yn * sin_a_;
238  double lnc = cos_a_ * ln;
239  double lns = sin_a_ * ln;
240 
241  if (vpFeatureSegment::selectXc() & select) {
242  vpMatrix Lxn(1, 6);
243  Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
244  Lxn[0][1] = lambda * xn * sin_a_;
245  Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
246  Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
247  Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
248  Lxn[0][5] = yn;
249  L = vpMatrix::stack(L, Lxn);
250  }
251 
252  if (vpFeatureSegment::selectYc() & select) {
253  vpMatrix Lyn(1, 6);
254  Lyn[0][0] = lambda * yn * cos_a_;
255  Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
256  Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
257  Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
258  Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
259  Lyn[0][5] = -xn;
260  L = vpMatrix::stack(L, Lyn);
261  }
262 
263  if (vpFeatureSegment::selectL() & select) {
264  vpMatrix Lln(1, 6);
265  Lln[0][0] = lambda * lnc;
266  Lln[0][1] = lambda * lns;
267  Lln[0][2] = -(Zn_inv + lambda * xnalpha);
268  Lln[0][3] = -yn - xnalpha * sin_a_;
269  Lln[0][4] = xn + xnalpha * cos_a_;
270  Lln[0][5] = 0;
271  L = vpMatrix::stack(L, Lln);
272  }
273  if (vpFeatureSegment::selectAlpha() & select) {
274  // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains
275  // 1/l
276  vpMatrix Lalpha(1, 6);
277  Lalpha[0][0] = -lambda1 * sin_a_ * l_;
278  Lalpha[0][1] = lambda1 * cos_a_ * l_;
279  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
280  Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
281  Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
282  Lalpha[0][5] = -1;
283  L = vpMatrix::stack(L, Lalpha);
284  }
285  }
286  else {
287  if (vpFeatureSegment::selectXc() & select) {
288  vpMatrix Lxc(1, 6);
289  Lxc[0][0] = -lambda2;
290  Lxc[0][1] = 0.;
291  Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
292  Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
293  Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
294  Lxc[0][5] = yc_;
295  L = vpMatrix::stack(L, Lxc);
296  }
297 
298  if (vpFeatureSegment::selectYc() & select) {
299  vpMatrix Lyc(1, 6);
300  Lyc[0][0] = 0.;
301  Lyc[0][1] = -lambda2;
302  Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
303  Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
304  Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
305  Lyc[0][5] = -xc_;
306  L = vpMatrix::stack(L, Lyc);
307  }
308 
309  if (vpFeatureSegment::selectL() & select) {
310  vpMatrix Ll(1, 6);
311  Ll[0][0] = lambda1 * cos_a_;
312  Ll[0][1] = lambda1 * sin_a_;
313  Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
314  Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
315  Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
316  Ll[0][5] = 0;
317  L = vpMatrix::stack(L, Ll);
318  }
319  if (vpFeatureSegment::selectAlpha() & select) {
320  vpMatrix Lalpha(1, 6);
321  Lalpha[0][0] = -lambda1 * sin_a_ / l_;
322  Lalpha[0][1] = lambda1 * cos_a_ / l_;
323  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
324  Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
325  Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
326  Lalpha[0][5] = -1;
327  L = vpMatrix::stack(L, Lalpha);
328  }
329  }
330 
331  return L;
332 }
333 
358 vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, unsigned int select)
359 {
360  vpColVector e(0);
361 
362  if (vpFeatureSegment::selectXc() & select) {
363  vpColVector exc(1);
364  exc[0] = xc_ - s_star[0];
365  e = vpColVector::stack(e, exc);
366  }
367 
368  if (vpFeatureSegment::selectYc() & select) {
369  vpColVector eyc(1);
370  eyc[0] = yc_ - s_star[1];
371  e = vpColVector::stack(e, eyc);
372  }
373 
374  if (vpFeatureSegment::selectL() & select) {
375  vpColVector eL(1);
376  eL[0] = l_ - s_star[2];
377  e = vpColVector::stack(e, eL);
378  }
379 
380  if (vpFeatureSegment::selectAlpha() & select) {
381  vpColVector eAlpha(1);
382  eAlpha[0] = alpha_ - s_star[3];
383  while (eAlpha[0] < -M_PI)
384  eAlpha[0] += 2 * M_PI;
385  while (eAlpha[0] > M_PI)
386  eAlpha[0] -= 2 * M_PI;
387  e = vpColVector::stack(e, eAlpha);
388  }
389  return e;
390 }
391 
419 void vpFeatureSegment::print(unsigned int select) const
420 {
421  std::cout << "vpFeatureSegment: (";
422  if (vpFeatureSegment::selectXc() & select) {
423  if (normalized_)
424  std::cout << "xn = ";
425  else
426  std::cout << "xc = ";
427  std::cout << s[0] << "; ";
428  }
429  if (vpFeatureSegment::selectYc() & select) {
430  if (normalized_)
431  std::cout << "yn = ";
432  else
433  std::cout << "yc = ";
434  std::cout << s[1] << "; ";
435  }
436  if (vpFeatureSegment::selectL() & select) {
437  if (normalized_)
438  std::cout << "ln = ";
439  else
440  std::cout << "l = ";
441  std::cout << s[2] << "; ";
442  }
443  if (vpFeatureSegment::selectAlpha() & select) {
444  std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
445  }
446  std::cout << ")" << std::endl;
447 }
448 
459 {
460  vpFeatureSegment *feature;
461 
462  feature = new vpFeatureSegment(*this);
463  return feature;
464 }
465 
476  unsigned int thickness) const
477 {
478  double l, x, y;
479  if (normalized_) {
480  l = 1. / l_;
481  x = xc_ * l;
482  y = yc_ * l;
483  }
484  else {
485  l = l_;
486  x = xc_;
487  y = yc_;
488  }
489 
490  double x1 = x - (l / 2.) * cos_a_;
491  double x2 = x + (l / 2.) * cos_a_;
492 
493  double y1 = y - (l / 2.) * sin_a_;
494  double y2 = y + (l / 2.) * sin_a_;
495  vpImagePoint ip1, ip2;
496 
497  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
498  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
499  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
500  vpDisplay::displayCircle(I, ip1, 5, color, true);
501  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
502 }
503 
513 void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
514  unsigned int thickness) const
515 {
516  double l, x, y;
517  if (normalized_) {
518  l = 1. / l_;
519  x = xc_ * l;
520  y = yc_ * l;
521  }
522  else {
523  l = l_;
524  x = xc_;
525  y = yc_;
526  }
527 
528  double x1 = x - (l / 2.) * cos_a_;
529  double x2 = x + (l / 2.) * cos_a_;
530 
531  double y1 = y - (l / 2.) * sin_a_;
532  double y2 = y + (l / 2.) * sin_a_;
533  vpImagePoint ip1, ip2;
534 
535  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
536  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
537  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
538  vpDisplay::displayCircle(I, ip1, 5, vpColor::cyan, true);
539  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
540 }
541 
558 vpFeatureSegment &vpFeatureSegment::buildFrom(const double &x1, const double &y1, const double &Z1, const double &x2, const double &y2, const double &Z2)
559 {
560  double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
561  double x_c = (x1 + x2) / 2.;
562  double y_c = (y1 + y2) / 2.;
563  double alpha = atan2(y1 - y2, x1 - x2);
564 
565  if (normalized_) {
566  setXc(x_c / l);
567  setYc(y_c / l);
568  setL(1 / l);
569  setAlpha(alpha);
570 
571  setZ1(Z1);
572  setZ2(Z2);
573  }
574  else {
575  setXc(x_c);
576  setYc(y_c);
577  setL(l);
578  setAlpha(alpha);
579 
580  setZ1(Z1);
581  setZ2(Z2);
582  }
583  return *this;
584 }
585 
608 unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
609 
632 unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
633 
656 unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
657 
680 unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3]; }
681 END_VISP_NAMESPACE
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor yellow
Definition: vpColor.h:225
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
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
void init() VP_OVERRIDE
static unsigned int selectAlpha()
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
static unsigned int selectXc()
void setAlpha(double val)
static unsigned int selectYc()
vpFeatureSegment * duplicate() const VP_OVERRIDE
Feature duplication.
void setL(double val)
void setZ2(double val)
VP_EXPLICIT vpFeatureSegment(bool normalized=false)
vpFeatureSegment & buildFrom(const double &x1, const double &y1, const double &Z1, const double &x2, const double &y2, const double &Z2)
void setZ1(double val)
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
void setXc(double val)
static unsigned int selectL()
void setYc(double val)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)