Visual Servoing Platform  version 3.6.1 under development (2024-04-16)
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 
34 #include <cmath>
35 #include <visp3/core/vpDisplay.h>
36 #include <visp3/core/vpImagePoint.h>
37 #include <visp3/core/vpMath.h>
38 #include <visp3/core/vpMeterPixelConversion.h>
39 #include <visp3/visual_features/vpBasicFeature.h>
40 #include <visp3/visual_features/vpFeatureSegment.h>
41 
42 // Exception
43 #include <visp3/core/vpException.h>
44 
45 // Debug trace
46 #include <visp3/core/vpDebug.h>
47 
57 {
58  // feature dimension
59  dim_s = 4;
60  nbParameters = 6;
61 
62  // memory allocation
63  s.resize(dim_s);
64  if (flags == nullptr)
65  flags = new bool[nbParameters];
66  for (unsigned int i = 0; i < nbParameters; i++)
67  flags[i] = false;
68 }
69 
78  : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
79 {
80  init();
81 }
82 
179 {
180  vpMatrix L;
181  L.resize(0, 6);
182 
184  for (unsigned int i = 0; i < nbParameters; i++) {
185  if (flags[i] == false) {
186  switch (i) {
187  case 0:
188  vpTRACE("Warning !!! The interaction matrix is computed but xc "
189  "was not set yet");
190  break;
191  case 1:
192  vpTRACE("Warning !!! The interaction matrix is computed but Yc "
193  "was not set yet");
194  break;
195  case 2:
196  vpTRACE("Warning !!! The interaction matrix is computed but l was "
197  "not set yet");
198  break;
199  case 3:
200  vpTRACE("Warning !!! The interaction matrix is computed but alpha "
201  "was not set yet");
202  break;
203  case 4:
204  vpTRACE("Warning !!! The interaction matrix is computed but Z1 "
205  "was not set yet");
206  break;
207  case 5:
208  vpTRACE("Warning !!! The interaction matrix is computed but Z2 "
209  "was not set yet");
210  break;
211  default:
212  vpTRACE("Problem during the reading of the variable flags");
213  }
214  }
215  }
216  }
217 
218  // This version is a simplification
219  double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_); // -l * lambda
220  double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_); // 1/Zm
221 
222  if (normalized_) {
223  // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
224  double xn = xc_;
225  double yn = yc_;
226  double ln = l_;
227  double lambda = -lambda1 * ln;
228  double Zn_inv = lambda2 * ln;
229  double lc = cos_a_ / ln;
230  double ls = sin_a_ / ln;
231  double xnalpha = xn * cos_a_ + yn * sin_a_;
232  double lnc = cos_a_ * ln;
233  double lns = sin_a_ * ln;
234 
235  if (vpFeatureSegment::selectXc() & select) {
236  vpMatrix Lxn(1, 6);
237  Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
238  Lxn[0][1] = lambda * xn * sin_a_;
239  Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
240  Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
241  Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
242  Lxn[0][5] = yn;
243  L = vpMatrix::stack(L, Lxn);
244  }
245 
246  if (vpFeatureSegment::selectYc() & select) {
247  vpMatrix Lyn(1, 6);
248  Lyn[0][0] = lambda * yn * cos_a_;
249  Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
250  Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
251  Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
252  Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
253  Lyn[0][5] = -xn;
254  L = vpMatrix::stack(L, Lyn);
255  }
256 
257  if (vpFeatureSegment::selectL() & select) {
258  vpMatrix Lln(1, 6);
259  Lln[0][0] = lambda * lnc;
260  Lln[0][1] = lambda * lns;
261  Lln[0][2] = -(Zn_inv + lambda * xnalpha);
262  Lln[0][3] = -yn - xnalpha * sin_a_;
263  Lln[0][4] = xn + xnalpha * cos_a_;
264  Lln[0][5] = 0;
265  L = vpMatrix::stack(L, Lln);
266  }
267  if (vpFeatureSegment::selectAlpha() & select) {
268  // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains
269  // 1/l
270  vpMatrix Lalpha(1, 6);
271  Lalpha[0][0] = -lambda1 * sin_a_ * l_;
272  Lalpha[0][1] = lambda1 * cos_a_ * l_;
273  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
274  Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
275  Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
276  Lalpha[0][5] = -1;
277  L = vpMatrix::stack(L, Lalpha);
278  }
279  }
280  else {
281  if (vpFeatureSegment::selectXc() & select) {
282  vpMatrix Lxc(1, 6);
283  Lxc[0][0] = -lambda2;
284  Lxc[0][1] = 0.;
285  Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
286  Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
287  Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
288  Lxc[0][5] = yc_;
289  L = vpMatrix::stack(L, Lxc);
290  }
291 
292  if (vpFeatureSegment::selectYc() & select) {
293  vpMatrix Lyc(1, 6);
294  Lyc[0][0] = 0.;
295  Lyc[0][1] = -lambda2;
296  Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
297  Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
298  Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
299  Lyc[0][5] = -xc_;
300  L = vpMatrix::stack(L, Lyc);
301  }
302 
303  if (vpFeatureSegment::selectL() & select) {
304  vpMatrix Ll(1, 6);
305  Ll[0][0] = lambda1 * cos_a_;
306  Ll[0][1] = lambda1 * sin_a_;
307  Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
308  Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
309  Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
310  Ll[0][5] = 0;
311  L = vpMatrix::stack(L, Ll);
312  }
313  if (vpFeatureSegment::selectAlpha() & select) {
314  vpMatrix Lalpha(1, 6);
315  Lalpha[0][0] = -lambda1 * sin_a_ / l_;
316  Lalpha[0][1] = lambda1 * cos_a_ / l_;
317  Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
318  Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
319  Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
320  Lalpha[0][5] = -1;
321  L = vpMatrix::stack(L, Lalpha);
322  }
323  }
324 
325  return L;
326 }
327 
352 vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, unsigned int select)
353 {
354  vpColVector e(0);
355 
356  if (vpFeatureSegment::selectXc() & select) {
357  vpColVector exc(1);
358  exc[0] = xc_ - s_star[0];
359  e = vpColVector::stack(e, exc);
360  }
361 
362  if (vpFeatureSegment::selectYc() & select) {
363  vpColVector eyc(1);
364  eyc[0] = yc_ - s_star[1];
365  e = vpColVector::stack(e, eyc);
366  }
367 
368  if (vpFeatureSegment::selectL() & select) {
369  vpColVector eL(1);
370  eL[0] = l_ - s_star[2];
371  e = vpColVector::stack(e, eL);
372  }
373 
374  if (vpFeatureSegment::selectAlpha() & select) {
375  vpColVector eAlpha(1);
376  eAlpha[0] = alpha_ - s_star[3];
377  while (eAlpha[0] < -M_PI)
378  eAlpha[0] += 2 * M_PI;
379  while (eAlpha[0] > M_PI)
380  eAlpha[0] -= 2 * M_PI;
381  e = vpColVector::stack(e, eAlpha);
382  }
383  return e;
384 }
385 
413 void vpFeatureSegment::print(unsigned int select) const
414 {
415  std::cout << "vpFeatureSegment: (";
416  if (vpFeatureSegment::selectXc() & select) {
417  if (normalized_)
418  std::cout << "xn = ";
419  else
420  std::cout << "xc = ";
421  std::cout << s[0] << "; ";
422  }
423  if (vpFeatureSegment::selectYc() & select) {
424  if (normalized_)
425  std::cout << "yn = ";
426  else
427  std::cout << "yc = ";
428  std::cout << s[1] << "; ";
429  }
430  if (vpFeatureSegment::selectL() & select) {
431  if (normalized_)
432  std::cout << "ln = ";
433  else
434  std::cout << "l = ";
435  std::cout << s[2] << "; ";
436  }
437  if (vpFeatureSegment::selectAlpha() & select) {
438  std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
439  }
440  std::cout << ")" << std::endl;
441 }
442 
453 {
454  vpFeatureSegment *feature;
455 
456  feature = new vpFeatureSegment(*this);
457  return feature;
458 }
459 
470  unsigned int thickness) const
471 {
472  double l, x, y;
473  if (normalized_) {
474  l = 1. / l_;
475  x = xc_ * l;
476  y = yc_ * l;
477  }
478  else {
479  l = l_;
480  x = xc_;
481  y = yc_;
482  }
483 
484  double x1 = x - (l / 2.) * cos_a_;
485  double x2 = x + (l / 2.) * cos_a_;
486 
487  double y1 = y - (l / 2.) * sin_a_;
488  double y2 = y + (l / 2.) * sin_a_;
489  vpImagePoint ip1, ip2;
490 
491  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
492  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
493  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
494  vpDisplay::displayCircle(I, ip1, 5, color, true);
495  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
496 }
497 
507 void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
508  unsigned int thickness) const
509 {
510  double l, x, y;
511  if (normalized_) {
512  l = 1. / l_;
513  x = xc_ * l;
514  y = yc_ * l;
515  }
516  else {
517  l = l_;
518  x = xc_;
519  y = yc_;
520  }
521 
522  double x1 = x - (l / 2.) * cos_a_;
523  double x2 = x + (l / 2.) * cos_a_;
524 
525  double y1 = y - (l / 2.) * sin_a_;
526  double y2 = y + (l / 2.) * sin_a_;
527  vpImagePoint ip1, ip2;
528 
529  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
530  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
531  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
532  vpDisplay::displayCircle(I, ip1, 5, vpColor::cyan, true);
533  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
534 }
535 
552 void vpFeatureSegment::buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
553 {
554  double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
555  double x_c = (x1 + x2) / 2.;
556  double y_c = (y1 + y2) / 2.;
557  double alpha = atan2(y1 - y2, x1 - x2);
558 
559  if (normalized_) {
560  setXc(x_c / l);
561  setYc(y_c / l);
562  setL(1 / l);
563  setAlpha(alpha);
564 
565  setZ1(Z1);
566  setZ2(Z2);
567  }
568  else {
569  setXc(x_c);
570  setYc(y_c);
571  setL(l);
572  setAlpha(alpha);
573 
574  setZ1(Z1);
575  setZ2(Z2);
576  }
577 }
578 
601 unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
602 
625 unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
626 
649 unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
650 
673 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:163
void stack(double d)
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
static const vpColor cyan
Definition: vpColor.h:220
static const vpColor yellow
Definition: vpColor.h:219
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 print(unsigned int select=FEATURE_ALL) const vp_override
static unsigned int selectAlpha()
void buildFrom(double x1, double y1, double Z1, double x2, double y2, double Z2)
static unsigned int selectXc()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
void setAlpha(double val)
static unsigned int selectYc()
void setL(double val)
void setZ2(double val)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
vpFeatureSegment(bool normalized=false)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
void setZ1(double val)
vpFeatureSegment * duplicate() const vp_override
Feature duplication.
void setXc(double val)
void init() vp_override
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:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5669
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpTRACE
Definition: vpDebug.h:405