Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpFeatureVanishingPoint.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 vanishing point visual feature (Z coordinate in 3D space is infinity)
33  *
34  * Authors:
35  * Odile Bourquardez
36  *
37 *****************************************************************************/
38 
43 #include <visp3/visual_features/vpBasicFeature.h>
44 #include <visp3/visual_features/vpFeatureVanishingPoint.h>
45 
46 // Exception
47 #include <visp3/core/vpException.h>
48 #include <visp3/visual_features/vpFeatureException.h>
49 
50 // Debug trace
51 #include <visp3/core/vpDebug.h>
52 
53 // math
54 #include <visp3/core/vpMath.h>
55 
56 #include <visp3/core/vpFeatureDisplay.h>
57 
58 BEGIN_VISP_NAMESPACE
63 {
64  // Feature dimension
65  dim_s = 5;
66  nbParameters = 5;
67  m_select = 0;
68 
69  // memory allocation
70  s.resize(dim_s);
71  if (flags == nullptr)
72  flags = new bool[nbParameters];
73  for (unsigned int i = 0; i < nbParameters; i++)
74  flags[i] = false;
75 }
76 
79 
82 {
83  s[0] = x;
84  flags[0] = true;
85  m_select |= selectX();
86 }
87 
89 double vpFeatureVanishingPoint::get_x() const { return s[0]; }
90 
93 {
94  s[1] = y;
95  flags[1] = true;
96  m_select |= selectY();
97 }
98 
100 double vpFeatureVanishingPoint::get_y() const { return s[1]; }
101 
103 void vpFeatureVanishingPoint::set_xy(double x, double y)
104 {
105  set_x(x);
106  set_y(y);
107  m_select = selectX() | selectY();
108 }
109 
111 void vpFeatureVanishingPoint::setOneOverRho(double one_over_rho)
112 {
113  s[2] = one_over_rho;
114  flags[2] = true;
116 }
117 
119 void vpFeatureVanishingPoint::setAtanOneOverRho(double atan_one_over_rho)
120 {
121  s[3] = atan_one_over_rho;
122  flags[3] = true;
124 }
125 
127 double vpFeatureVanishingPoint::getOneOverRho() const { return s[2]; }
128 
130 double vpFeatureVanishingPoint::getAtanOneOverRho() const { return s[3]; }
131 
134 {
135  s[4] = alpha;
136  flags[4] = true;
137  m_select |= selectAlpha();
138 }
139 
141 double vpFeatureVanishingPoint::getAlpha() const { return s[4]; }
142 
152 {
153  vpMatrix L;
154 
155  L.resize(0, 6);
156 
158  for (unsigned int i = 0; i < nbParameters; i++) {
159  if (flags[i] == false) {
160  switch (i) {
161  case 0:
162  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
163  break;
164  case 1:
165  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
166  break;
167  case 2:
168  vpTRACE("Warning !!! The interaction matrix is computed but 1/rho was not set yet");
169  break;
170  case 3:
171  vpTRACE("Warning !!! The interaction matrix is computed but atan(1/rho) was not set yet");
172  break;
173  case 4:
174  vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
175  break;
176  default:
177  vpTRACE("Problem during the reading of the variable flags");
178  }
179  }
180  }
181  resetFlags();
182  }
183 
184  if (vpFeatureVanishingPoint::selectX() & select) {
185  double x = get_x();
186  double y = get_y();
187  vpMatrix Lx(1, 6);
188  Lx = 0;
189 
190  Lx[0][0] = 0.;
191  Lx[0][1] = 0.;
192  Lx[0][2] = 0.;
193  Lx[0][3] = x * y;
194  Lx[0][4] = -(1 + x * x);
195  Lx[0][5] = y;
196 
197  L = vpMatrix::stack(L, Lx);
198  }
199 
200  if (vpFeatureVanishingPoint::selectY() & select) {
201  double x = get_x();
202  double y = get_y();
203  vpMatrix Ly(1, 6);
204  Ly = 0;
205 
206  Ly[0][0] = 0;
207  Ly[0][1] = 0.;
208  Ly[0][2] = 0.;
209  Ly[0][3] = 1 + y * y;
210  Ly[0][4] = -x * y;
211  Ly[0][5] = -x;
212 
213  L = vpMatrix::stack(L, Ly);
214  }
215 
217  double one_over_rho = getOneOverRho();
218  double alpha = getAlpha();
219  vpMatrix Lone_over_rho(1, 6);
220  double rho2 = 1. + one_over_rho * one_over_rho;
221 
222  Lone_over_rho[0][0] = 0.;
223  Lone_over_rho[0][1] = 0.;
224  Lone_over_rho[0][2] = 0.;
225  Lone_over_rho[0][3] = -rho2 * sin(alpha);
226  Lone_over_rho[0][4] = rho2 * cos(alpha);
227  Lone_over_rho[0][5] = 0.;
228 
229  L = vpMatrix::stack(L, Lone_over_rho);
230  }
231 
233  double alpha = getAlpha();
234  vpMatrix Latan_one_over_rho(1, 6);
235 
236  Latan_one_over_rho[0][0] = 0.;
237  Latan_one_over_rho[0][1] = 0.;
238  Latan_one_over_rho[0][2] = 0.;
239  Latan_one_over_rho[0][3] = -sin(alpha);
240  Latan_one_over_rho[0][4] = cos(alpha);
241  Latan_one_over_rho[0][5] = 0.;
242 
243  L = vpMatrix::stack(L, Latan_one_over_rho);
244  }
245 
246  if (vpFeatureVanishingPoint::selectAlpha() & select) {
247  double one_over_rho = getOneOverRho();
248  double alpha = getAlpha();
249  vpMatrix Lalpha(1, 6);
250 
251  Lalpha[0][0] = 0;
252  Lalpha[0][1] = 0.;
253  Lalpha[0][2] = 0.;
254  Lalpha[0][3] = cos(alpha) * one_over_rho;
255  Lalpha[0][4] = sin(alpha) * one_over_rho;
256  Lalpha[0][5] = -1.;
257 
258  L = vpMatrix::stack(L, Lalpha);
259  }
260 
261  return L;
262 }
263 
275 vpColVector vpFeatureVanishingPoint::error(const vpBasicFeature &s_star, unsigned int select)
276 {
277  vpColVector e(0);
278 
279  if (vpFeatureVanishingPoint::selectX() & select) {
280  vpColVector ex(1);
281  ex[0] = s[0] - s_star[0];
282 
283  e = vpColVector::stack(e, ex);
284  }
285 
286  if (vpFeatureVanishingPoint::selectY() & select) {
287  vpColVector ey(1);
288  ey[0] = s[1] - s_star[1];
289  e = vpColVector::stack(e, ey);
290  }
291 
293  vpColVector e_one_over_rho(1);
294  e_one_over_rho[0] = s[2] - s_star[2];
295 
296  e = vpColVector::stack(e, e_one_over_rho);
297  }
298 
300  vpColVector e_atan_one_over_rho(1);
301  e_atan_one_over_rho[0] = s[3] - s_star[3];
302 
303  e = vpColVector::stack(e, e_atan_one_over_rho);
304  }
305 
306  if (vpFeatureVanishingPoint::selectAlpha() & select) {
307  vpColVector e_alpha(1);
308  double err = s[4] - s_star[4];
309 
310  if (err < -M_PI)
311  err += 2 * M_PI;
312  if (err > M_PI)
313  err -= 2 * M_PI;
314 
315  e_alpha[0] = err;
316  e = vpColVector::stack(e, e_alpha);
317  }
318 
319  return e;
320 }
321 
330 void vpFeatureVanishingPoint::print(unsigned int select) const
331 {
332  std::cout << "Vanishing point:";
333  if (vpFeatureVanishingPoint::selectX() & select)
334  std::cout << " x=" << get_x();
335  if (vpFeatureVanishingPoint::selectY() & select)
336  std::cout << " y=" << get_y();
338  std::cout << " 1/rho=" << getOneOverRho();
339  }
341  std::cout << " atan(1/rho)=" << getAtanOneOverRho();
342  }
343  if (vpFeatureVanishingPoint::selectAlpha() & select) {
344  std::cout << " alpha=" << getAlpha();
345  }
346  std::cout << std::endl;
347 }
348 
351 {
352  set_xy(x, y);
353  return *this;
354 }
355 
365  const vpColor &color, unsigned int thickness) const
366 {
368  double x, y;
369  x = get_x();
370  y = get_y();
371 
372  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
373  }
375  double one_over_rho = getOneOverRho();
376  double alpha = getAlpha();
377  double x = cos(alpha) / one_over_rho;
378  double y = sin(alpha) / one_over_rho;
379  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
380  }
382  double atan_one_over_rho = getAtanOneOverRho();
383  double alpha = getAlpha();
384  double x = cos(alpha) / tan(atan_one_over_rho);
385  double y = sin(alpha) / tan(atan_one_over_rho);
386  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
387  }
388 }
389 
399  unsigned int thickness) const
400 {
402  double x, y;
403  x = get_x();
404  y = get_y();
405 
406  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
407  }
409  double one_over_rho = getOneOverRho();
410  double alpha = getAlpha();
411  double x = cos(alpha) / one_over_rho;
412  double y = sin(alpha) / one_over_rho;
413  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
414  }
416  double atan_one_over_rho = getAtanOneOverRho();
417  double alpha = getAlpha();
418  double x = cos(alpha) / tan(atan_one_over_rho);
419  double y = sin(alpha) / tan(atan_one_over_rho);
420  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
421  }
422 }
423 
428 {
430  return feature;
431 }
432 
434 unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0]; }
435 
437 unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1]; }
438 
443 
448 
453 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 displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
static unsigned int selectAtanOneOverRho()
vpFeatureVanishingPoint * duplicate() const VP_OVERRIDE
void set_y(double y)
Set vanishing point feature value.
void setAlpha(double alpha)
Set vanishing point feature value.
double getOneOverRho() const
Get vanishing point feature value.
double getAlpha() const
Get vanishing point feature value.
void setAtanOneOverRho(double atan_one_over_rho)
Set vanishing point feature value.
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(vpFeatureVanishingPoint::selectX()|vpFeatureVanishingPoint::selectY())) VP_OVERRIDE
void setOneOverRho(double one_over_rho)
Set vanishing point feature value.
double getAtanOneOverRho() const
Get vanishing point feature value.
void set_xy(double x, double y)
Set vanishing point visual feature from cartesian coordinates. Same as buildFrom().
static unsigned int selectX()
Select visual feature .
static unsigned int selectOneOverRho()
vpFeatureVanishingPoint & buildFrom(const double &x, const double &y)
Set vanishing point visual feature from cartesian coordinates. Same as set_xy().
double get_y() const
Get vanishing point feature value.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const VP_OVERRIDE
static unsigned int selectAlpha()
vpMatrix interaction(unsigned int select=(vpFeatureVanishingPoint::selectX()|vpFeatureVanishingPoint::selectY())) VP_OVERRIDE
double get_x() const
Get vanishing point feature value.
vpFeatureVanishingPoint()
Default constructor that calls init().
static unsigned int selectY()
Select visual feature .
void print(unsigned int select=(vpFeatureVanishingPoint::selectX()|vpFeatureVanishingPoint::selectY())) const VP_OVERRIDE
void set_x(double x)
Set vanishing point feature value.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)