Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
vpFeatureLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeatureLine.h>
43 
44 // Exception
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
51 // simple math function (round)
52 #include <visp3/core/vpMath.h>
53 
54 // Display Issue
55 
56 // Meter/pixel conversion
57 #include <visp3/core/vpCameraParameters.h>
58 
59 // Color / image / display
60 #include <visp3/core/vpColor.h>
61 #include <visp3/core/vpImage.h>
62 
63 #include <visp3/core/vpFeatureDisplay.h>
64 
65 /*
66 attributes and members directly related to the vpBasicFeature needs
67 other functionalities ar useful but not mandatory
68 */
69 
74 {
75  // feature dimension
76  dim_s = 2;
77  nbParameters = 6;
78 
79  // memory allocation
80  // x cos(theta) + y sin(theta) - rho = 0
81  // s[0] = rho
82  // s[1] = theta
83  s.resize(dim_s);
84  if (flags == nullptr)
85  flags = new bool[nbParameters];
86  for (unsigned int i = 0; i < nbParameters; i++)
87  flags[i] = false;
88 
89  A = B = C = D = 0.0;
90 }
91 
95 vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
96 
104 void vpFeatureLine::setRhoTheta(double rho, double theta)
105 {
106  s[0] = rho;
107  s[1] = theta;
108  for (int i = 0; i < 2; i++)
109  flags[i] = true;
110 }
111 
126 void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
127 {
128  this->A = A_;
129  this->B = B_;
130  this->C = C_;
131  this->D = D_;
132  for (unsigned int i = 2; i < nbParameters; i++)
133  flags[i] = true;
134 }
135 
188 {
189  vpMatrix L;
190 
192  for (unsigned int i = 0; i < nbParameters; i++) {
193  if (flags[i] == false) {
194  switch (i) {
195  case 0:
196  vpTRACE("Warning !!! The interaction matrix is computed but rho "
197  "was not set yet");
198  break;
199  case 1:
200  vpTRACE("Warning !!! The interaction matrix is computed but theta "
201  "was not set yet");
202  break;
203  case 2:
204  vpTRACE("Warning !!! The interaction matrix is computed but A was "
205  "not set yet");
206  break;
207  case 3:
208  vpTRACE("Warning !!! The interaction matrix is computed but B was "
209  "not set yet");
210  break;
211  case 4:
212  vpTRACE("Warning !!! The interaction matrix is computed but C was "
213  "not set yet");
214  break;
215  case 5:
216  vpTRACE("Warning !!! The interaction matrix is computed but D was "
217  "not set yet");
218  break;
219  default:
220  vpTRACE("Problem during the reading of the variable flags");
221  }
222  }
223  }
224  resetFlags();
225  }
226  double rho = s[0];
227  double theta = s[1];
228 
229  double co = cos(theta);
230  double si = sin(theta);
231 
232  if (fabs(D) < 1e-6) {
233  vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
234 
235  throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
236  }
237 
238  double lambda_theta = (A * si - B * co) / D;
239  double lambda_rho = (C + rho * A * co + rho * B * si) / D;
240 
241  if (vpFeatureLine::selectRho() & select) {
242  vpMatrix Lrho(1, 6);
243 
244  Lrho[0][0] = co * lambda_rho;
245  Lrho[0][1] = si * lambda_rho;
246  Lrho[0][2] = -rho * lambda_rho;
247  Lrho[0][3] = si * (1.0 + rho * rho);
248  Lrho[0][4] = -co * (1.0 + rho * rho);
249  Lrho[0][5] = 0.0;
250 
251  L.stack(Lrho);
252  }
253 
254  if (vpFeatureLine::selectTheta() & select) {
255  vpMatrix Ltheta(1, 6);
256 
257  Ltheta[0][0] = co * lambda_theta;
258  Ltheta[0][1] = si * lambda_theta;
259  Ltheta[0][2] = -rho * lambda_theta;
260  Ltheta[0][3] = -rho * co;
261  Ltheta[0][4] = -rho * si;
262  Ltheta[0][5] = -1.0;
263 
264  L.stack(Ltheta);
265  }
266  return L;
267 }
268 
307 vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
308 {
309  vpColVector e(0);
310 
311  try {
312  if (vpFeatureLine::selectRho() & select) {
313  vpColVector erho(1);
314  erho[0] = s[0] - s_star[0];
315 
316  e = vpColVector::stack(e, erho);
317  }
318 
319  if (vpFeatureLine::selectTheta() & select) {
320 
321  double err = s[1] - s_star[1];
322  while (err < -M_PI)
323  err += 2 * M_PI;
324  while (err > M_PI)
325  err -= 2 * M_PI;
326 
327  vpColVector etheta(1);
328  etheta[0] = err;
329  e = vpColVector::stack(e, etheta);
330  }
331  } catch (...) {
332  throw;
333  }
334 
335  return e;
336 }
337 
358 void vpFeatureLine::print(unsigned int select) const
359 {
360 
361  std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
362  ;
363  if (vpFeatureLine::selectRho() & select)
364  std::cout << " \trho=" << s[0];
365  if (vpFeatureLine::selectTheta() & select)
366  std::cout << " \ttheta=" << s[1];
367  std::cout << std::endl;
368 }
369 
384 void vpFeatureLine::buildFrom(double rho, double theta)
385 {
386  s[0] = rho;
387  s[1] = theta;
388  for (int i = 0; i < 2; i++)
389  flags[i] = true;
390 }
391 
420 void vpFeatureLine::buildFrom(double rho, double theta, double A_, double B_, double C_, double D_)
421 {
422  s[0] = rho;
423  s[1] = theta;
424  this->A = A_;
425  this->B = B_;
426  this->C = C_;
427  this->D = D_;
428  for (unsigned int i = 0; i < nbParameters; i++)
429  flags[i] = true;
430 }
431 
443 {
444  vpFeatureLine *feature = new vpFeatureLine;
445  return feature;
446 }
447 
459  unsigned int thickness) const
460 {
461  try {
462  double rho, theta;
463  rho = getRho();
464  theta = getTheta();
465 
466  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
467 
468  } catch (...) {
469  vpERROR_TRACE("Error caught");
470  throw;
471  }
472 }
473 
484 void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
485  unsigned int thickness) const
486 {
487  try {
488  double rho, theta;
489  rho = getRho();
490  theta = getTheta();
491 
492  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
493 
494  } catch (...) {
495  vpERROR_TRACE("Error caught");
496  throw;
497  }
498 }
499 
518 unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
519 
539 unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
void setRhoTheta(double rho, double theta)
void print(unsigned int select=FEATURE_ALL) const vp_override
double getTheta() const
vpFeatureLine * duplicate() const vp_override
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
static unsigned int selectRho()
void init() vp_override
void buildFrom(double rho, double theta)
void setABCD(double A, double B, double C, double D)
static unsigned int selectTheta()
double getRho() const
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382