ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpFeatureLine.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureLine.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  * 2D line visual feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeatureLine.h>
51 
52 // Exception
53 #include <visp/vpException.h>
54 #include <visp/vpMatrixException.h>
55 #include <visp/vpFeatureException.h>
56 
57 // Debug trace
58 #include <visp/vpDebug.h>
59 
60 // simple math function (round)
61 #include <visp/vpMath.h>
62 
63 // Display Issue
64 
65 // Meter/pixel conversion
66 #include <visp/vpCameraParameters.h>
67 
68 //Color / image / display
69 #include <visp/vpColor.h>
70 #include <visp/vpImage.h>
71 
72 
73 
74 #include <visp/vpFeatureDisplay.h>
75 
76 
77 /*
78 
79 
80 
81 attributes and members directly related to the vpBasicFeature needs
82 other functionalities ar useful but not mandatory
83 
84 
85 
86 
87 
88 */
89 
93 void
95 {
96  //feature dimension
97  dim_s = 2 ;
98  nbParameters = 6;
99 
100  // memory allocation
101  // x cos(theta) + y sin(theta) - rho = 0
102  // s[0] = rho
103  // s[1] = theta
104  s.resize(dim_s) ;
105  if (flags == NULL)
106  flags = new bool[nbParameters];
107  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
108 
109  A = B = C = D = 0.0 ;
110 }
111 
112 
117 {
118  init() ;
119 }
120 
121 
128 void
129 vpFeatureLine::setRhoTheta(const double rho, const double theta)
130 {
131  s[0] = rho ;
132  s[1] = theta ;
133  for( int i = 0; i < 2; i++) flags[i] = true;
134 }
135 
136 
147 void
148 vpFeatureLine::setABCD(const double A, const double B,
149  const double C, const double D)
150 {
151  this->A = A ;
152  this->B = B ;
153  this->C = C ;
154  this->D = D ;
155  for(unsigned int i = 2; i < nbParameters; i++) flags[i] = true;
156 }
157 
158 
207 vpMatrix
208 vpFeatureLine::interaction(const unsigned int select)
209 {
210  vpMatrix L ;
211 
212  L.resize(0,6) ;
213 
215  {
216  for (unsigned int i = 0; i < nbParameters; i++)
217  {
218  if (flags[i] == false)
219  {
220  switch(i){
221  case 0:
222  vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
223  break;
224  case 1:
225  vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
226  break;
227  case 2:
228  vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
229  break;
230  case 3:
231  vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
232  break;
233  case 4:
234  vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
235  break;
236  case 5:
237  vpTRACE("Warning !!! The interaction matrix is computed but D was not set yet");
238  break;
239  default:
240  vpTRACE("Problem during the reading of the variable flags");
241  }
242  }
243  }
244  resetFlags();
245  }
246  double rho = s[0] ;
247  double theta = s[1] ;
248 
249 
250  double co = cos(theta);
251  double si = sin(theta);
252 
253  if (fabs(D) < 1e-6)
254  {
255  vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f",D) ;
256 
258  "Incorrect plane coordinates D")) ;
259  }
260 
261  double lambda_theta =( A*si - B*co) /D;
262  double lambda_rho = (C + rho*A*co + rho*B*si)/D;
263 
264  if (vpFeatureLine::selectRho() & select )
265  {
266  vpMatrix Lrho(1,6) ;
267 
268 
269  Lrho[0][0]= co*lambda_rho;
270  Lrho[0][1]= si*lambda_rho;
271  Lrho[0][2]= -rho*lambda_rho;
272  Lrho[0][3]= si*(1.0 + rho*rho);
273  Lrho[0][4]= -co*(1.0 + rho*rho);
274  Lrho[0][5]= 0.0;
275 
276  L = vpMatrix::stackMatrices(L,Lrho) ;
277  }
278 
279  if (vpFeatureLine::selectTheta() & select )
280  {
281  vpMatrix Ltheta(1,6) ;
282 
283  Ltheta[0][0] = co*lambda_theta;
284  Ltheta[0][1] = si*lambda_theta;
285  Ltheta[0][2] = -rho*lambda_theta;
286  Ltheta[0][3] = -rho*co;
287  Ltheta[0][4] = -rho*si;
288  Ltheta[0][5] = -1.0;
289 
290  L = vpMatrix::stackMatrices(L,Ltheta) ;
291  }
292  return L ;
293 }
294 
295 
336  const unsigned int select)
337 {
338  vpColVector e(0) ;
339 
340  try{
341  if (vpFeatureLine::selectRho() & select )
342  {
343  vpColVector erho(1) ;
344  erho[0] = s[0] - s_star[0] ;
345 
346 
347 
348  e = vpMatrix::stackMatrices(e,erho) ;
349  }
350 
351  if (vpFeatureLine::selectTheta() & select )
352  {
353 
354  double err = s[1] - s_star[1] ;
355  while (err < -M_PI) err += 2*M_PI ;
356  while (err > M_PI) err -= 2*M_PI ;
357 
358  vpColVector etheta(1) ;
359  etheta[0] = err ;
360  e = vpMatrix::stackMatrices(e,etheta) ;
361  }
362  }
363  catch(vpMatrixException me)
364  {
365  vpERROR_TRACE("caught a Matric related error") ;
366  std::cout <<std::endl << me << std::endl ;
367  throw(me) ;
368  }
369  catch(vpException me)
370  {
371  vpERROR_TRACE("caught another error") ;
372  std::cout <<std::endl << me << std::endl ;
373  throw(me) ;
374  }
375 
376 
377  return e ;
378 
379 }
380 
381 
402 void
403 vpFeatureLine::print(const unsigned int select ) const
404 {
405 
406  std::cout <<"Line:\t " << A <<"X+" << B <<"Y+" << C <<"Z +" << D <<"=0" <<std::endl ;;
407  if (vpFeatureLine::selectRho() & select )
408  std::cout << " \trho=" << s[0] ;
409  if (vpFeatureLine::selectTheta() & select )
410  std::cout << " \ttheta=" << s[1] ;
411  std::cout <<std::endl ;
412 }
413 
414 
429 void
430 vpFeatureLine::buildFrom(const double rho, const double theta)
431 {
432  s[0] = rho ;
433  s[1] = theta ;
434  for( int i = 0; i < 2; i++) flags[i] = true;
435 }
436 
437 
466 void vpFeatureLine::buildFrom(const double rho, const double theta,
467  const double A, const double B,
468  const double C, const double D)
469 {
470  s[0] = rho ;
471  s[1] = theta ;
472  this->A = A ;
473  this->B = B ;
474  this->C = C ;
475  this->D = D ;
476  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
477 }
478 
479 
491 {
492  vpFeatureLine *feature = new vpFeatureLine ;
493  return feature ;
494 }
495 
496 
497 
508 void
510  const vpImage<unsigned char> &I,
511  const vpColor &color,
512  unsigned int thickness) const
513 {
514  try{
515  double rho,theta ;
516  rho = getRho() ;
517  theta = getTheta() ;
518 
519  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
520 
521  }
522  catch(...)
523  {
524  vpERROR_TRACE("Error caught") ;
525  throw ;
526  }
527 }
528 
539 void
541  const vpImage<vpRGBa> &I,
542  const vpColor &color,
543  unsigned int thickness) const
544 {
545  try{
546  double rho,theta ;
547  rho = getRho() ;
548  theta = getTheta() ;
549 
550  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
551 
552  }
553  catch(...)
554  {
555  vpERROR_TRACE("Error caught") ;
556  throw ;
557  }
558 }
559 
560 
561 
562 /*
563  * Local variables:
564  * c-basic-offset: 2
565  * End:
566  */