Visual Servoing Platform  version 3.3.1 under development (2020-07-03)
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 attributes and members directly related to the vpBasicFeature needs
70 other functionalities ar useful but not mandatory
71 */
72 
77 {
78  // feature dimension
79  dim_s = 2;
80  nbParameters = 6;
81 
82  // memory allocation
83  // x cos(theta) + y sin(theta) - rho = 0
84  // s[0] = rho
85  // s[1] = theta
86  s.resize(dim_s);
87  if (flags == NULL)
88  flags = new bool[nbParameters];
89  for (unsigned int i = 0; i < nbParameters; i++)
90  flags[i] = false;
91 
92  A = B = C = D = 0.0;
93 }
94 
98 vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
99 
107 void vpFeatureLine::setRhoTheta(double rho, double theta)
108 {
109  s[0] = rho;
110  s[1] = theta;
111  for (int i = 0; i < 2; i++)
112  flags[i] = true;
113 }
114 
129 void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
130 {
131  this->A = A_;
132  this->B = B_;
133  this->C = C_;
134  this->D = D_;
135  for (unsigned int i = 2; i < nbParameters; i++)
136  flags[i] = true;
137 }
138 
191 {
192  vpMatrix L;
193 
195  for (unsigned int i = 0; i < nbParameters; i++) {
196  if (flags[i] == false) {
197  switch (i) {
198  case 0:
199  vpTRACE("Warning !!! The interaction matrix is computed but rho "
200  "was not set yet");
201  break;
202  case 1:
203  vpTRACE("Warning !!! The interaction matrix is computed but theta "
204  "was not set yet");
205  break;
206  case 2:
207  vpTRACE("Warning !!! The interaction matrix is computed but A was "
208  "not set yet");
209  break;
210  case 3:
211  vpTRACE("Warning !!! The interaction matrix is computed but B was "
212  "not set yet");
213  break;
214  case 4:
215  vpTRACE("Warning !!! The interaction matrix is computed but C was "
216  "not set yet");
217  break;
218  case 5:
219  vpTRACE("Warning !!! The interaction matrix is computed but D was "
220  "not set yet");
221  break;
222  default:
223  vpTRACE("Problem during the reading of the variable flags");
224  }
225  }
226  }
227  resetFlags();
228  }
229  double rho = s[0];
230  double theta = s[1];
231 
232  double co = cos(theta);
233  double si = sin(theta);
234 
235  if (fabs(D) < 1e-6) {
236  vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
237 
238  throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
239  }
240 
241  double lambda_theta = (A * si - B * co) / D;
242  double lambda_rho = (C + rho * A * co + rho * B * si) / D;
243 
244  if (vpFeatureLine::selectRho() & select) {
245  vpMatrix Lrho(1, 6);
246 
247  Lrho[0][0] = co * lambda_rho;
248  Lrho[0][1] = si * lambda_rho;
249  Lrho[0][2] = -rho * lambda_rho;
250  Lrho[0][3] = si * (1.0 + rho * rho);
251  Lrho[0][4] = -co * (1.0 + rho * rho);
252  Lrho[0][5] = 0.0;
253 
254  L.stack(Lrho);
255  }
256 
257  if (vpFeatureLine::selectTheta() & select) {
258  vpMatrix Ltheta(1, 6);
259 
260  Ltheta[0][0] = co * lambda_theta;
261  Ltheta[0][1] = si * lambda_theta;
262  Ltheta[0][2] = -rho * lambda_theta;
263  Ltheta[0][3] = -rho * co;
264  Ltheta[0][4] = -rho * si;
265  Ltheta[0][5] = -1.0;
266 
267  L.stack(Ltheta);
268  }
269  return L;
270 }
271 
310 vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
311 {
312  vpColVector e(0);
313 
314  try {
315  if (vpFeatureLine::selectRho() & select) {
316  vpColVector erho(1);
317  erho[0] = s[0] - s_star[0];
318 
319  e = vpColVector::stack(e, erho);
320  }
321 
322  if (vpFeatureLine::selectTheta() & select) {
323 
324  double err = s[1] - s_star[1];
325  while (err < -M_PI)
326  err += 2 * M_PI;
327  while (err > M_PI)
328  err -= 2 * M_PI;
329 
330  vpColVector etheta(1);
331  etheta[0] = err;
332  e = vpColVector::stack(e, etheta);
333  }
334  } catch (...) {
335  throw;
336  }
337 
338  return e;
339 }
340 
361 void vpFeatureLine::print(unsigned int select) const
362 {
363 
364  std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
365  ;
366  if (vpFeatureLine::selectRho() & select)
367  std::cout << " \trho=" << s[0];
368  if (vpFeatureLine::selectTheta() & select)
369  std::cout << " \ttheta=" << s[1];
370  std::cout << std::endl;
371 }
372 
387 void vpFeatureLine::buildFrom(double rho, double theta)
388 {
389  s[0] = rho;
390  s[1] = theta;
391  for (int i = 0; i < 2; i++)
392  flags[i] = true;
393 }
394 
423 void vpFeatureLine::buildFrom(double rho, double theta, double A_, double B_, double C_, double D_)
424 {
425  s[0] = rho;
426  s[1] = theta;
427  this->A = A_;
428  this->B = B_;
429  this->C = C_;
430  this->D = D_;
431  for (unsigned int i = 0; i < nbParameters; i++)
432  flags[i] = true;
433 }
434 
446 {
447  vpFeatureLine *feature = new vpFeatureLine;
448  return feature;
449 }
450 
462  unsigned int thickness) const
463 {
464  try {
465  double rho, theta;
466  rho = getRho();
467  theta = getTheta();
468 
469  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
470 
471  } catch (...) {
472  vpERROR_TRACE("Error caught");
473  throw;
474  }
475 }
476 
487 void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
488  unsigned int thickness) const
489 {
490  try {
491  double rho, theta;
492  rho = getRho();
493  theta = getTheta();
494 
495  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
496 
497  } catch (...) {
498  vpERROR_TRACE("Error caught");
499  throw;
500  }
501 }
502 
521 unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
522 
542 unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
void setABCD(double A, double B, double C, double D)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix interaction(unsigned int select=FEATURE_ALL)
static unsigned int selectRho()
vpFeatureLine * duplicate() const
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4620
double getRho() const
unsigned int dim_s
Dimension of the visual feature.
void buildFrom(double rho, double theta)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
class that defines what is a visual feature
#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 ...
double getTheta() const
void setRhoTheta(double rho, double theta)
void print(unsigned int select=FEATURE_ALL) const
static const unsigned int FEATURE_LINE[32]
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpBasicFeatureDeallocatorType deallocate
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
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)
static unsigned int selectTheta()
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.