Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureSegment.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Filip Novotny
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
40 #include <visp3/visual_features/vpBasicFeature.h>
41 #include <visp3/visual_features/vpFeatureSegment.h>
42 #include <visp3/core/vpImagePoint.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpDisplay.h>
46 #include <cmath>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 
51 // Debug trace
52 #include <visp3/core/vpDebug.h>
53 
54 
66 void
68 {
69  //feature dimension
70  dim_s = 4 ;
71  nbParameters = 6;
72 
73  // memory allocation
74  s.resize(dim_s) ;
75  if (flags == NULL)
76  flags = new bool[nbParameters];
77  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
78 }
79 
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
189 vpFeatureSegment::interaction( const unsigned int select )
190 {
191 
192  vpMatrix L ;
193  L.resize(0,6) ;
194 
196  {
197  for (unsigned int i = 0; i < nbParameters; i++)
198  {
199  if (flags[i] == false)
200  {
201  switch(i){
202  case 0:
203  vpTRACE("Warning !!! The interaction matrix is computed but xc was not set yet");
204  break;
205  case 1:
206  vpTRACE("Warning !!! The interaction matrix is computed but Yc was not set yet");
207  break;
208  case 2:
209  vpTRACE("Warning !!! The interaction matrix is computed but l was not set yet");
210  break;
211  case 3:
212  vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
213  break;
214  case 4:
215  vpTRACE("Warning !!! The interaction matrix is computed but Z1 was not set yet");
216  break;
217  case 5:
218  vpTRACE("Warning !!! The interaction matrix is computed but Z2 was not set yet");
219  break;
220  default:
221  vpTRACE("Problem during the reading of the variable flags");
222  }
223  }
224  }
225  }
226 
227  //This version is a simplification
228  double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
229  double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
230 
231  if (normalized_)
232  {
233  // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
234  double xn = xc_;
235  double yn = yc_;
236  double ln = l_;
237  double lambda = -lambda1 * ln;
238  double Zn_inv = lambda2 * ln;
239  double lc = cos_a_ / ln;
240  double ls = sin_a_ / ln;
241  double xnalpha = xn*cos_a_+yn*sin_a_;
242  double lnc = cos_a_ * ln;
243  double lns = sin_a_ * ln;
244 
245  if (vpFeatureSegment::selectXc() & select ){
246  vpMatrix Lxn(1,6);
247  Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
248  Lxn[0][1] = lambda * xn * sin_a_ ;
249  Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
250  Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
251  Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
252  Lxn[0][5] = yn ;
253  L = vpMatrix::stack(L, Lxn) ;
254  }
255 
256  if (vpFeatureSegment::selectYc() & select ){
257  vpMatrix Lyn(1,6);
258  Lyn[0][0] = lambda*yn*cos_a_ ;
259  Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
260  Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
261  Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
262  Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
263  Lyn[0][5] = -xn ;
264  L = vpMatrix::stack(L, Lyn) ;
265  }
266 
267  if (vpFeatureSegment::selectL() & select ){
268  vpMatrix Lln(1,6);
269  Lln[0][0] = lambda * lnc ;
270  Lln[0][1] = lambda * lns ;
271  Lln[0][2] = -(Zn_inv + lambda*xnalpha);
272  Lln[0][3] = -yn-xnalpha*sin_a_ ;
273  Lln[0][4] = xn + xnalpha*cos_a_ ;
274  Lln[0][5] = 0 ;
275  L = vpMatrix::stack(L, Lln) ;
276  }
277  if (vpFeatureSegment::selectAlpha() & select ){
278  // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
279  vpMatrix Lalpha(1,6);
280  Lalpha[0][0] = -lambda1*sin_a_*l_ ;
281  Lalpha[0][1] = lambda1*cos_a_*l_ ;
282  Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_);
283  Lalpha[0][3] = (-xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_)/l_;
284  Lalpha[0][4] = (xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_)/l_ ;
285  Lalpha[0][5] = -1 ;
286  L = vpMatrix::stack(L,Lalpha) ;
287  }
288  }
289  else
290  {
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 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) eAlpha[0] += 2*M_PI ;
389  while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
390  e = vpColVector::stack(e,eAlpha) ;
391  }
392  return e ;
393 }
394 
421 void
422 vpFeatureSegment::print( const unsigned int select ) const
423 {
424  std::cout <<"vpFeatureSegment: (";
425  if (vpFeatureSegment::selectXc() & select ) {
426  if (normalized_)
427  std::cout << "xn = ";
428  else
429  std::cout << "xc = ";
430  std::cout << s[0] << "; ";
431  }
432  if (vpFeatureSegment::selectYc() & select ) {
433  if (normalized_)
434  std::cout << "yn = ";
435  else
436  std::cout << "yc = ";
437  std::cout << s[1] << "; ";
438  }
439  if (vpFeatureSegment::selectL() & select ) {
440  if (normalized_)
441  std::cout << "ln = ";
442  else
443  std::cout << "l = ";
444  std::cout << s[2] << "; ";
445  }
446  if (vpFeatureSegment::selectAlpha() & select ) {
447  std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
448  }
449  std::cout << ")" << std::endl;
450 }
451 
463 {
464  vpFeatureSegment *feature;
465 
466  feature = new vpFeatureSegment(*this) ;
467  return feature ;
468 }
469 
481 void
483  const vpImage<unsigned char> & I,
484  const vpColor &color,
485  unsigned int thickness ) const
486 {
487  double l, x, y;
488  if (normalized_) {
489  l = 1./l_;
490  x = xc_ * l;
491  y = yc_ * l;
492  }
493  else {
494  l = l_;
495  x = xc_;
496  y = yc_;
497  }
498 
499  double x1 = x - (l/2.)*cos_a_;
500  double x2 = x + (l/2.)*cos_a_;
501 
502  double y1 = y - (l/2.)*sin_a_;
503  double y2 = y + (l/2.)*sin_a_;
504  vpImagePoint ip1,ip2;
505 
506  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
507  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
508  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
509  vpDisplay::displayCircle(I, ip1, 5, color, true);
510  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
511 }
512 
523 void
525  const vpImage<vpRGBa> & I,
526  const vpColor &color,
527  unsigned int thickness ) const
528 {
529  double l, x, y;
530  if (normalized_) {
531  l = 1./l_;
532  x = xc_ * l;
533  y = yc_ * l;
534  }
535  else {
536  l = l_;
537  x = xc_;
538  y = yc_;
539  }
540 
541  double x1 = x - (l/2.)*cos_a_;
542  double x2 = x + (l/2.)*cos_a_;
543 
544  double y1 = y - (l/2.)*sin_a_;
545  double y2 = y + (l/2.)*sin_a_;
546  vpImagePoint ip1,ip2;
547 
548  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
549  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
550  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
551  vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
552  vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
553 }
554 
573 void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
574  const double x2, const double y2, const double Z2)
575 {
576  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
577  double x_c = (x1+x2)/2.;
578  double y_c = (y1+y2)/2.;
579  double alpha = atan2(y1-y2,x1-x2);
580 
581  if (normalized_) {
582  setXc( x_c / l );
583  setYc( y_c / l );
584  setL( 1/l );
585  setAlpha( alpha );
586 
587  setZ1(Z1);
588  setZ2(Z2);
589  }
590  else {
591  setXc( x_c );
592  setYc( y_c );
593  setL( l );
594  setAlpha( alpha );
595 
596  setZ1(Z1);
597  setZ2(Z2);
598  }
599 }
600 
623 unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0] ; }
624 
647 unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1] ; }
648 
671 unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2] ; }
672 
696 unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
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()
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
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:172
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:414
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:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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:171
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:225
void setXc(const double val)
void print(const unsigned int select=FEATURE_ALL) const