ViSP  2.10.0
vpFeatureSegment.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureThetaU.cpp 3530 2012-01-03 10:52:12Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Segment visual feature.
36  *
37  * Authors:
38  * Filip Novotny
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
44 #include <visp/vpBasicFeature.h>
45 #include <visp/vpFeatureSegment.h>
46 #include <visp/vpImagePoint.h>
47 #include <visp/vpMeterPixelConversion.h>
48 #include <visp/vpMath.h>
49 #include <visp/vpDisplay.h>
50 #include <cmath>
51 
52 // Exception
53 #include <visp/vpException.h>
54 #include <visp/vpMatrixException.h>
55 
56 // Debug trace
57 #include <visp/vpDebug.h>
58 
59 
71 void
73 {
74  //feature dimension
75  dim_s = 4 ;
76  nbParameters = 6;
77 
78  // memory allocation
79  s.resize(dim_s) ;
80  if (flags == NULL)
81  flags = new bool[nbParameters];
82  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
83 }
84 
92  : xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
93 {
94  init();
95 }
96 
195 vpMatrix
196 vpFeatureSegment::interaction( const unsigned int select )
197 {
198 
199  vpMatrix L ;
200  L.resize(0,6) ;
201 
203  {
204  for (unsigned int i = 0; i < nbParameters; i++)
205  {
206  if (flags[i] == false)
207  {
208  switch(i){
209  case 0:
210  vpTRACE("Warning !!! The interaction matrix is computed but xc was not set yet");
211  break;
212  case 1:
213  vpTRACE("Warning !!! The interaction matrix is computed but Yc was not set yet");
214  break;
215  case 2:
216  vpTRACE("Warning !!! The interaction matrix is computed but l was not set yet");
217  break;
218  case 3:
219  vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
220  break;
221  case 4:
222  vpTRACE("Warning !!! The interaction matrix is computed but Z1 was not set yet");
223  break;
224  case 5:
225  vpTRACE("Warning !!! The interaction matrix is computed but Z2 was not set yet");
226  break;
227  default:
228  vpTRACE("Problem during the reading of the variable flags");
229  }
230  }
231  }
232  }
233 
234  //This version is a simplification
235  double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
236  double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
237 
238  if (normalized_)
239  {
240  // here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
241  double xn = xc_;
242  double yn = yc_;
243  double ln = l_;
244  double lambda = -lambda1 * ln;
245  double Zn_inv = lambda2 * ln;
246  double lc = cos_a_ / ln;
247  double ls = sin_a_ / ln;
248  double xnalpha = xn*cos_a_+yn*sin_a_;
249  double lnc = cos_a_ * ln;
250  double lns = sin_a_ * ln;
251 
252  if (vpFeatureSegment::selectXc() & select ){
253  vpMatrix Lxn(1,6);
254  Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
255  Lxn[0][1] = lambda * xn * sin_a_ ;
256  Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
257  Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
258  Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
259  Lxn[0][5] = yn ;
260  L = vpMatrix::stackMatrices(L, Lxn) ;
261  }
262 
263  if (vpFeatureSegment::selectYc() & select ){
264  vpMatrix Lyn(1,6);
265  Lyn[0][0] = lambda*yn*cos_a_ ;
266  Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
267  Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
268  Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
269  Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
270  Lyn[0][5] = -xn ;
271  L = vpMatrix::stackMatrices(L, Lyn) ;
272  }
273 
274  if (vpFeatureSegment::selectL() & select ){
275  vpMatrix Lln(1,6);
276  Lln[0][0] = lambda * lnc ;
277  Lln[0][1] = lambda * lns ;
278  Lln[0][2] = -(Zn_inv + lambda*xnalpha);
279  Lln[0][3] = -yn-xnalpha*sin_a_ ;
280  Lln[0][4] = xn + xnalpha*cos_a_ ;
281  Lln[0][5] = 0 ;
282  L = vpMatrix::stackMatrices(L, Lln) ;
283  }
284  if (vpFeatureSegment::selectAlpha() & select ){
285  // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
286  vpMatrix Lalpha(1,6);
287  Lalpha[0][0] = -lambda1*sin_a_*l_ ;
288  Lalpha[0][1] = lambda1*cos_a_*l_ ;
289  Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_);
290  Lalpha[0][3] = (-xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_)/l_;
291  Lalpha[0][4] = (xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_)/l_ ;
292  Lalpha[0][5] = -1 ;
293  L = vpMatrix::stackMatrices(L,Lalpha) ;
294  }
295  }
296  else
297  {
298  if (vpFeatureSegment::selectXc() & select ){
299  vpMatrix Lxc(1,6);
300  Lxc[0][0] = -lambda2 ;
301  Lxc[0][1] = 0. ;
302  Lxc[0][2] = lambda2*xc_ - lambda1*l_*cos_a_/4.;
303  Lxc[0][3] = xc_*yc_ + l_*l_*cos_a_*sin_a_/4. ;
304  Lxc[0][4] = -(1+xc_*xc_+l_*l_*cos_a_*cos_a_/4.) ;
305  Lxc[0][5] = yc_ ;
306  L = vpMatrix::stackMatrices(L,Lxc) ;
307  }
308 
309  if (vpFeatureSegment::selectYc() & select ){
310  vpMatrix Lyc(1,6);
311  Lyc[0][0] = 0. ;
312  Lyc[0][1] = -lambda2 ;
313  Lyc[0][2] = lambda2*yc_ - lambda1*l_*sin_a_/4.;
314  Lyc[0][3] = 1+yc_*yc_+l_*l_*sin_a_*sin_a_/4. ;
315  Lyc[0][4] = -xc_*yc_-l_*l_*cos_a_*sin_a_/4. ;
316  Lyc[0][5] = -xc_ ;
317  L = vpMatrix::stackMatrices(L,Lyc) ;
318  }
319 
320  if (vpFeatureSegment::selectL() & select ){
321  vpMatrix Ll(1,6);
322  Ll[0][0] = lambda1*cos_a_ ;
323  Ll[0][1] = lambda1*sin_a_ ;
324  Ll[0][2] = lambda2*l_-lambda1*(xc_*cos_a_+yc_*sin_a_);
325  Ll[0][3] = l_*(xc_*cos_a_*sin_a_ + yc_*(1+sin_a_*sin_a_)) ;
326  Ll[0][4] = -l_*(xc_*(1+cos_a_*cos_a_)+yc_*cos_a_*sin_a_) ;
327  Ll[0][5] = 0 ;
328  L = vpMatrix::stackMatrices(L,Ll) ;
329  }
330  if (vpFeatureSegment::selectAlpha() & select ){
331  vpMatrix Lalpha(1,6);
332  Lalpha[0][0] = -lambda1*sin_a_/l_ ;
333  Lalpha[0][1] = lambda1*cos_a_/l_ ;
334  Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_)/l_;
335  Lalpha[0][3] = -xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_;
336  Lalpha[0][4] = xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_ ;
337  Lalpha[0][5] = -1 ;
338  L = vpMatrix::stackMatrices(L,Lalpha) ;
339  }
340  }
341 
342  return L ;
343 }
344 
370 vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select )
371 {
372  vpColVector e(0) ;
373 
374  if (vpFeatureSegment::selectXc() & select ){
375  vpColVector exc(1) ;
376  exc[0] = xc_-s_star[0];
377  e = vpMatrix::stackMatrices(e,exc) ;
378  }
379 
380  if (vpFeatureSegment::selectYc() & select ){
381  vpColVector eyc(1) ;
382  eyc[0] = yc_ - s_star[1];
383  e = vpMatrix::stackMatrices(e,eyc) ;
384  }
385 
386  if (vpFeatureSegment::selectL() & select ){
387  vpColVector eL(1) ;
388  eL[0] = l_ - s_star[2];
389  e = vpMatrix::stackMatrices(e,eL) ;
390  }
391 
392  if (vpFeatureSegment::selectAlpha() & select ){
393  vpColVector eAlpha(1) ;
394  eAlpha[0] = alpha_ - s_star[3];
395  while (eAlpha[0] < -M_PI) eAlpha[0] += 2*M_PI ;
396  while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
397  e = vpMatrix::stackMatrices(e,eAlpha) ;
398  }
399  return e ;
400 }
401 
428 void
429 vpFeatureSegment::print( const unsigned int select ) const
430 {
431  std::cout <<"vpFeatureSegment: (";
432  if (vpFeatureSegment::selectXc() & select ) {
433  if (normalized_)
434  std::cout << "xn = ";
435  else
436  std::cout << "xc = ";
437  std::cout << s[0] << "; ";
438  }
439  if (vpFeatureSegment::selectYc() & select ) {
440  if (normalized_)
441  std::cout << "yn = ";
442  else
443  std::cout << "yc = ";
444  std::cout << s[1] << "; ";
445  }
446  if (vpFeatureSegment::selectL() & select ) {
447  if (normalized_)
448  std::cout << "ln = ";
449  else
450  std::cout << "l = ";
451  std::cout << s[2] << "; ";
452  }
453  if (vpFeatureSegment::selectAlpha() & select ) {
454  std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
455  }
456  std::cout << ")" << std::endl;
457 }
458 
470 {
471  vpFeatureSegment *feature;
472 
473  feature = new vpFeatureSegment(*this) ;
474  return feature ;
475 }
476 
488 void
490  const vpImage<unsigned char> & I,
491  const vpColor &color,
492  unsigned int thickness ) const
493 {
494  double l, x, y;
495  if (normalized_) {
496  l = 1./l_;
497  x = xc_ * l;
498  y = yc_ * l;
499  }
500  else {
501  l = l_;
502  x = xc_;
503  y = yc_;
504  }
505 
506  double x1 = x - (l/2.)*cos_a_;
507  double x2 = x + (l/2.)*cos_a_;
508 
509  double y1 = y - (l/2.)*sin_a_;
510  double y2 = y + (l/2.)*sin_a_;
511  vpImagePoint ip1,ip2;
512 
513  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
514  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
515  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
516  vpDisplay::displayCircle(I, ip1, 5, color, true);
517  vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
518 }
519 
530 void
532  const vpImage<vpRGBa> & I,
533  const vpColor &color,
534  unsigned int thickness ) const
535 {
536  double l, x, y;
537  if (normalized_) {
538  l = 1./l_;
539  x = xc_ * l;
540  y = yc_ * l;
541  }
542  else {
543  l = l_;
544  x = xc_;
545  y = yc_;
546  }
547 
548  double x1 = x - (l/2.)*cos_a_;
549  double x2 = x + (l/2.)*cos_a_;
550 
551  double y1 = y - (l/2.)*sin_a_;
552  double y2 = y + (l/2.)*sin_a_;
553  vpImagePoint ip1,ip2;
554 
555  vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
556  vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
557  vpDisplay::displayLine(I, ip1, ip2, color, thickness);
558  vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
559  vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
560 }
561 
580 void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
581  const double x2, const double y2, const double Z2)
582 {
583  double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
584  double x_c = (x1+x2)/2.;
585  double y_c = (y1+y2)/2.;
586  double alpha = atan2(y1-y2,x1-x2);
587 
588  if (normalized_) {
589  setXc( x_c / l );
590  setYc( y_c / l );
591  setL( 1/l );
592  setAlpha( alpha );
593 
594  setZ1(Z1);
595  setZ2(Z2);
596  }
597  else {
598  setXc( x_c );
599  setYc( y_c );
600  setL( l );
601  setAlpha( alpha );
602 
603  setZ1(Z1);
604  setZ2(Z2);
605  }
606 }
607 
608 
virtual void displayCircle(const vpImagePoint &center, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)=0
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
static unsigned int selectL()
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:199
void setAlpha(const double val)
#define vpTRACE
Definition: vpDebug.h:418
static unsigned int selectYc()
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:125
static unsigned int selectAlpha()
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:176
void setZ2(const double val)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
Generic class defining intrinsic camera parameters.
vpFeatureSegment(bool normalized=false)
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3272
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)
vpBasicFeatureDeallocatorType deallocate
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
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:93
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static const vpColor yellow
Definition: vpColor.h:175
vpFeatureSegment * duplicate() const
Feature duplication.
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.
static unsigned int selectXc()
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98
void setXc(const double val)
void print(const unsigned int select=FEATURE_ALL) const