Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
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:1049
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 init() override
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)
vpMatrix interaction(unsigned int select=FEATURE_ALL) override
vpFeatureSegment * duplicate() const override
Feature duplication.
void print(unsigned int select=FEATURE_ALL) const override
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const override
vpFeatureSegment(bool normalized=false)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) override
void setZ1(double val)
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:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5873
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpTRACE
Definition: vpDebug.h:411