Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
vpFeatureLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * 2D line visual feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
44 #include <visp3/visual_features/vpBasicFeature.h>
45 #include <visp3/visual_features/vpFeatureLine.h>
46 
47 // Exception
48 #include <visp3/core/vpException.h>
49 #include <visp3/visual_features/vpFeatureException.h>
50 
51 // Debug trace
52 #include <visp3/core/vpDebug.h>
53 
54 // simple math function (round)
55 #include <visp3/core/vpMath.h>
56 
57 // Display Issue
58 
59 // Meter/pixel conversion
60 #include <visp3/core/vpCameraParameters.h>
61 
62 // Color / image / display
63 #include <visp3/core/vpColor.h>
64 #include <visp3/core/vpImage.h>
65 
66 #include <visp3/core/vpFeatureDisplay.h>
67 
68 /*
69 
70 
71 
72 attributes and members directly related to the vpBasicFeature needs
73 other functionalities ar useful but not mandatory
74 
75 
76 
77 
78 
79 */
80 
85 {
86  // feature dimension
87  dim_s = 2;
88  nbParameters = 6;
89 
90  // memory allocation
91  // x cos(theta) + y sin(theta) - rho = 0
92  // s[0] = rho
93  // s[1] = theta
94  s.resize(dim_s);
95  if (flags == NULL)
96  flags = new bool[nbParameters];
97  for (unsigned int i = 0; i < nbParameters; i++)
98  flags[i] = false;
99 
100  A = B = C = D = 0.0;
101 }
102 
106 vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
107 
115 void vpFeatureLine::setRhoTheta(const double rho, const double theta)
116 {
117  s[0] = rho;
118  s[1] = theta;
119  for (int i = 0; i < 2; i++)
120  flags[i] = true;
121 }
122 
137 void vpFeatureLine::setABCD(const double A_, const double B_, const double C_, const double D_)
138 {
139  this->A = A_;
140  this->B = B_;
141  this->C = C_;
142  this->D = D_;
143  for (unsigned int i = 2; i < nbParameters; i++)
144  flags[i] = true;
145 }
146 
198 vpMatrix vpFeatureLine::interaction(const unsigned int select)
199 {
200  vpMatrix L;
201 
203  for (unsigned int i = 0; i < nbParameters; i++) {
204  if (flags[i] == false) {
205  switch (i) {
206  case 0:
207  vpTRACE("Warning !!! The interaction matrix is computed but rho "
208  "was not set yet");
209  break;
210  case 1:
211  vpTRACE("Warning !!! The interaction matrix is computed but theta "
212  "was not set yet");
213  break;
214  case 2:
215  vpTRACE("Warning !!! The interaction matrix is computed but A was "
216  "not set yet");
217  break;
218  case 3:
219  vpTRACE("Warning !!! The interaction matrix is computed but B was "
220  "not set yet");
221  break;
222  case 4:
223  vpTRACE("Warning !!! The interaction matrix is computed but C was "
224  "not set yet");
225  break;
226  case 5:
227  vpTRACE("Warning !!! The interaction matrix is computed but D was "
228  "not set yet");
229  break;
230  default:
231  vpTRACE("Problem during the reading of the variable flags");
232  }
233  }
234  }
235  resetFlags();
236  }
237  double rho = s[0];
238  double theta = s[1];
239 
240  double co = cos(theta);
241  double si = sin(theta);
242 
243  if (fabs(D) < 1e-6) {
244  vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
245 
246  throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
247  }
248 
249  double lambda_theta = (A * si - B * co) / D;
250  double lambda_rho = (C + rho * A * co + rho * B * si) / D;
251 
252  if (vpFeatureLine::selectRho() & select) {
253  vpMatrix Lrho(1, 6);
254 
255  Lrho[0][0] = co * lambda_rho;
256  Lrho[0][1] = si * lambda_rho;
257  Lrho[0][2] = -rho * lambda_rho;
258  Lrho[0][3] = si * (1.0 + rho * rho);
259  Lrho[0][4] = -co * (1.0 + rho * rho);
260  Lrho[0][5] = 0.0;
261 
262  L.stack(Lrho);
263  }
264 
265  if (vpFeatureLine::selectTheta() & select) {
266  vpMatrix Ltheta(1, 6);
267 
268  Ltheta[0][0] = co * lambda_theta;
269  Ltheta[0][1] = si * lambda_theta;
270  Ltheta[0][2] = -rho * lambda_theta;
271  Ltheta[0][3] = -rho * co;
272  Ltheta[0][4] = -rho * si;
273  Ltheta[0][5] = -1.0;
274 
275  L.stack(Ltheta);
276  }
277  return L;
278 }
279 
318 vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, const unsigned int select)
319 {
320  vpColVector e(0);
321 
322  try {
323  if (vpFeatureLine::selectRho() & select) {
324  vpColVector erho(1);
325  erho[0] = s[0] - s_star[0];
326 
327  e = vpColVector::stack(e, erho);
328  }
329 
330  if (vpFeatureLine::selectTheta() & select) {
331 
332  double err = s[1] - s_star[1];
333  while (err < -M_PI)
334  err += 2 * M_PI;
335  while (err > M_PI)
336  err -= 2 * M_PI;
337 
338  vpColVector etheta(1);
339  etheta[0] = err;
340  e = vpColVector::stack(e, etheta);
341  }
342  } catch (...) {
343  throw;
344  }
345 
346  return e;
347 }
348 
369 void vpFeatureLine::print(const unsigned int select) const
370 {
371 
372  std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
373  ;
374  if (vpFeatureLine::selectRho() & select)
375  std::cout << " \trho=" << s[0];
376  if (vpFeatureLine::selectTheta() & select)
377  std::cout << " \ttheta=" << s[1];
378  std::cout << std::endl;
379 }
380 
395 void vpFeatureLine::buildFrom(const double rho, const double theta)
396 {
397  s[0] = rho;
398  s[1] = theta;
399  for (int i = 0; i < 2; i++)
400  flags[i] = true;
401 }
402 
431 void vpFeatureLine::buildFrom(const double rho, const double theta, const double A_, const double B_, const double C_,
432  const double D_)
433 {
434  s[0] = rho;
435  s[1] = theta;
436  this->A = A_;
437  this->B = B_;
438  this->C = C_;
439  this->D = D_;
440  for (unsigned int i = 0; i < nbParameters; i++)
441  flags[i] = true;
442 }
443 
455 {
456  vpFeatureLine *feature = new vpFeatureLine;
457  return feature;
458 }
459 
471  unsigned int thickness) const
472 {
473  try {
474  double rho, theta;
475  rho = getRho();
476  theta = getTheta();
477 
478  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
479 
480  } catch (...) {
481  vpERROR_TRACE("Error caught");
482  throw;
483  }
484 }
485 
496 void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
497  unsigned int thickness) const
498 {
499  try {
500  double rho, theta;
501  rho = getRho();
502  theta = getTheta();
503 
504  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
505 
506  } catch (...) {
507  vpERROR_TRACE("Error caught");
508  throw;
509  }
510 }
511 
530 unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
531 
551 unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void setABCD(const double A, const double B, const double C, const double D)
static unsigned int selectRho()
void print(const unsigned int select=FEATURE_ALL) const
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4551
unsigned int dim_s
Dimension of the visual feature.
double getRho() const
double getTheta() const
class that defines what is a visual feature
void buildFrom(const double rho, const double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
#define vpTRACE
Definition: vpDebug.h:416
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
vpFeatureLine * duplicate() const
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
static unsigned int selectTheta()
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:310
void setRhoTheta(const double rho, const double theta)