Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
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 
70 BEGIN_VISP_NAMESPACE
75 {
76  // feature dimension
77  dim_s = 2;
78  nbParameters = 6;
79 
80  // memory allocation
81  // x cos(theta) + y sin(theta) - rho = 0
82  // s[0] = rho
83  // s[1] = theta
84  s.resize(dim_s);
85  if (flags == nullptr)
86  flags = new bool[nbParameters];
87  for (unsigned int i = 0; i < nbParameters; i++)
88  flags[i] = false;
89 
90  A = B = C = D = 0.0;
91 }
92 
96 vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
97 
105 void vpFeatureLine::setRhoTheta(double rho, double theta)
106 {
107  s[0] = rho;
108  s[1] = theta;
109  for (int i = 0; i < 2; i++)
110  flags[i] = true;
111 }
112 
127 void vpFeatureLine::setABCD(double A_, double B_, double C_, double D_)
128 {
129  this->A = A_;
130  this->B = B_;
131  this->C = C_;
132  this->D = D_;
133  for (unsigned int i = 2; i < nbParameters; i++)
134  flags[i] = true;
135 }
136 
189 {
190  vpMatrix L;
191 
193  for (unsigned int i = 0; i < nbParameters; i++) {
194  if (flags[i] == false) {
195  switch (i) {
196  case 0:
197  vpTRACE("Warning !!! The interaction matrix is computed but rho "
198  "was not set yet");
199  break;
200  case 1:
201  vpTRACE("Warning !!! The interaction matrix is computed but theta "
202  "was not set yet");
203  break;
204  case 2:
205  vpTRACE("Warning !!! The interaction matrix is computed but A was "
206  "not set yet");
207  break;
208  case 3:
209  vpTRACE("Warning !!! The interaction matrix is computed but B was "
210  "not set yet");
211  break;
212  case 4:
213  vpTRACE("Warning !!! The interaction matrix is computed but C was "
214  "not set yet");
215  break;
216  case 5:
217  vpTRACE("Warning !!! The interaction matrix is computed but D was "
218  "not set yet");
219  break;
220  default:
221  vpTRACE("Problem during the reading of the variable flags");
222  }
223  }
224  }
225  resetFlags();
226  }
227  double rho = s[0];
228  double theta = s[1];
229 
230  double co = cos(theta);
231  double si = sin(theta);
232 
233  if (fabs(D) < 1e-6) {
234  vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f", D);
235 
236  throw(vpFeatureException(vpFeatureException::badInitializationError, "Incorrect plane coordinates D"));
237  }
238 
239  double lambda_theta = (A * si - B * co) / D;
240  double lambda_rho = (C + rho * A * co + rho * B * si) / D;
241 
242  if (vpFeatureLine::selectRho() & select) {
243  vpMatrix Lrho(1, 6);
244 
245  Lrho[0][0] = co * lambda_rho;
246  Lrho[0][1] = si * lambda_rho;
247  Lrho[0][2] = -rho * lambda_rho;
248  Lrho[0][3] = si * (1.0 + rho * rho);
249  Lrho[0][4] = -co * (1.0 + rho * rho);
250  Lrho[0][5] = 0.0;
251 
252  L.stack(Lrho);
253  }
254 
255  if (vpFeatureLine::selectTheta() & select) {
256  vpMatrix Ltheta(1, 6);
257 
258  Ltheta[0][0] = co * lambda_theta;
259  Ltheta[0][1] = si * lambda_theta;
260  Ltheta[0][2] = -rho * lambda_theta;
261  Ltheta[0][3] = -rho * co;
262  Ltheta[0][4] = -rho * si;
263  Ltheta[0][5] = -1.0;
264 
265  L.stack(Ltheta);
266  }
267  return L;
268 }
269 
308 vpColVector vpFeatureLine::error(const vpBasicFeature &s_star, unsigned int select)
309 {
310  vpColVector e(0);
311 
312  try {
313  if (vpFeatureLine::selectRho() & select) {
314  vpColVector erho(1);
315  erho[0] = s[0] - s_star[0];
316 
317  e = vpColVector::stack(e, erho);
318  }
319 
320  if (vpFeatureLine::selectTheta() & select) {
321 
322  double err = s[1] - s_star[1];
323  while (err < -M_PI)
324  err += 2 * M_PI;
325  while (err > M_PI)
326  err -= 2 * M_PI;
327 
328  vpColVector etheta(1);
329  etheta[0] = err;
330  e = vpColVector::stack(e, etheta);
331  }
332  }
333  catch (...) {
334  throw;
335  }
336 
337  return e;
338 }
339 
360 void vpFeatureLine::print(unsigned int select) const
361 {
362 
363  std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
364  ;
365  if (vpFeatureLine::selectRho() & select)
366  std::cout << " \trho=" << s[0];
367  if (vpFeatureLine::selectTheta() & select)
368  std::cout << " \ttheta=" << s[1];
369  std::cout << std::endl;
370 }
371 
386 vpFeatureLine &vpFeatureLine::buildFrom(const double &rho, const double &theta)
387 {
388  s[0] = rho;
389  s[1] = theta;
390  for (int i = 0; i < 2; i++) {
391  flags[i] = true;
392  }
393  return *this;
394 }
395 
424 vpFeatureLine &vpFeatureLine::buildFrom(const double &rho, const double &theta, const double &A_, const double &B_, const double &C_, const double &D_)
425 {
426  s[0] = rho;
427  s[1] = theta;
428  this->A = A_;
429  this->B = B_;
430  this->C = C_;
431  this->D = D_;
432  for (unsigned int i = 0; i < nbParameters; ++i) {
433  flags[i] = true;
434  }
435  return *this;
436 }
437 
449 {
450  vpFeatureLine *feature = new vpFeatureLine;
451  return feature;
452 }
453 
465  unsigned int thickness) const
466 {
467  try {
468  double rho, theta;
469  rho = getRho();
470  theta = getTheta();
471 
472  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
473 
474  }
475  catch (...) {
476  vpERROR_TRACE("Error caught");
477  throw;
478  }
479 }
480 
491 void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
492  unsigned int thickness) const
493 {
494  try {
495  double rho, theta;
496  rho = getRho();
497  theta = getTheta();
498 
499  vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
500 
501  }
502  catch (...) {
503  vpERROR_TRACE("Error caught");
504  throw;
505  }
506 }
507 
526 unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
527 
547 unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
548 END_VISP_NAMESPACE
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
unsigned int dim_s
Dimension of the visual feature.
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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 ,...
void init() VP_OVERRIDE
void setRhoTheta(double rho, double theta)
double getTheta() const
static unsigned int selectRho()
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
vpFeatureLine & buildFrom(const double &rho, const double &theta)
vpFeatureLine * duplicate() const VP_OVERRIDE
void setABCD(double A, double B, double C, double D)
static unsigned int selectTheta()
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
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
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169