Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpFeatureVanishingPoint.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 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 
62 {
63  // Feature dimension
64  dim_s = 5;
65  nbParameters = 5;
66  m_select = 0;
67 
68  // memory allocation
69  s.resize(dim_s);
70  if (flags == NULL)
71  flags = new bool[nbParameters];
72  for (unsigned int i = 0; i < nbParameters; i++)
73  flags[i] = false;
74 }
75 
78 
81 {
82  s[0] = x;
83  flags[0] = true;
84  m_select |= selectX();
85 }
86 
88 double vpFeatureVanishingPoint::get_x() const { return s[0]; }
89 
92 {
93  s[1] = y;
94  flags[1] = true;
95  m_select |= selectY();
96 }
97 
99 double vpFeatureVanishingPoint::get_y() const { return s[1]; }
100 
102 void vpFeatureVanishingPoint::set_xy(double x, double y)
103 {
104  set_x(x);
105  set_y(y);
106  m_select = selectX() | selectY();
107 }
108 
110 void vpFeatureVanishingPoint::setOneOverRho(double one_over_rho)
111 {
112  s[2] = one_over_rho;
113  flags[2] = true;
115 }
116 
118 void vpFeatureVanishingPoint::setAtanOneOverRho(double atan_one_over_rho)
119 {
120  s[3] = atan_one_over_rho;
121  flags[3] = true;
123 }
124 
126 double vpFeatureVanishingPoint::getOneOverRho() const { return s[2]; }
127 
129 double vpFeatureVanishingPoint::getAtanOneOverRho() const { return s[3]; }
130 
133 {
134  s[4] = alpha;
135  flags[4] = true;
136  m_select |= selectAlpha();
137 }
138 
140 double vpFeatureVanishingPoint::getAlpha() const { return s[4]; }
141 
151 {
152  vpMatrix L;
153 
154  L.resize(0, 6);
155 
157  for (unsigned int i = 0; i < nbParameters; i++) {
158  if (flags[i] == false) {
159  switch (i) {
160  case 0:
161  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
162  break;
163  case 1:
164  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
165  break;
166  case 2:
167  vpTRACE("Warning !!! The interaction matrix is computed but 1/rho was not set yet");
168  break;
169  case 3:
170  vpTRACE("Warning !!! The interaction matrix is computed but atan(1/rho) was not set yet");
171  break;
172  case 4:
173  vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
174  break;
175  default:
176  vpTRACE("Problem during the reading of the variable flags");
177  }
178  }
179  }
180  resetFlags();
181  }
182 
183  if (vpFeatureVanishingPoint::selectX() & select) {
184  double x = get_x();
185  double y = get_y();
186  vpMatrix Lx(1, 6);
187  Lx = 0;
188 
189  Lx[0][0] = 0.;
190  Lx[0][1] = 0.;
191  Lx[0][2] = 0.;
192  Lx[0][3] = x * y;
193  Lx[0][4] = -(1 + x * x);
194  Lx[0][5] = y;
195 
196  L = vpMatrix::stack(L, Lx);
197  }
198 
199  if (vpFeatureVanishingPoint::selectY() & select) {
200  double x = get_x();
201  double y = get_y();
202  vpMatrix Ly(1, 6);
203  Ly = 0;
204 
205  Ly[0][0] = 0;
206  Ly[0][1] = 0.;
207  Ly[0][2] = 0.;
208  Ly[0][3] = 1 + y * y;
209  Ly[0][4] = -x * y;
210  Ly[0][5] = -x;
211 
212  L = vpMatrix::stack(L, Ly);
213  }
214 
216  double one_over_rho = getOneOverRho();
217  double alpha = getAlpha();
218  vpMatrix Lone_over_rho(1, 6);
219  double rho2 = 1. + one_over_rho * one_over_rho;
220 
221  Lone_over_rho[0][0] = 0.;
222  Lone_over_rho[0][1] = 0.;
223  Lone_over_rho[0][2] = 0.;
224  Lone_over_rho[0][3] = - rho2 * sin(alpha);
225  Lone_over_rho[0][4] = rho2 * cos(alpha);
226  Lone_over_rho[0][5] = 0.;
227 
228  L = vpMatrix::stack(L, Lone_over_rho);
229  }
230 
232  double alpha = getAlpha();
233  vpMatrix Latan_one_over_rho(1, 6);
234 
235  Latan_one_over_rho[0][0] = 0.;
236  Latan_one_over_rho[0][1] = 0.;
237  Latan_one_over_rho[0][2] = 0.;
238  Latan_one_over_rho[0][3] = - sin(alpha);
239  Latan_one_over_rho[0][4] = cos(alpha);
240  Latan_one_over_rho[0][5] = 0.;
241 
242  L = vpMatrix::stack(L, Latan_one_over_rho);
243  }
244 
245  if (vpFeatureVanishingPoint::selectAlpha() & select) {
246  double one_over_rho = getOneOverRho();
247  double alpha = getAlpha();
248  vpMatrix Lalpha(1, 6);
249 
250  Lalpha[0][0] = 0;
251  Lalpha[0][1] = 0.;
252  Lalpha[0][2] = 0.;
253  Lalpha[0][3] = cos(alpha) * one_over_rho;
254  Lalpha[0][4] = sin(alpha) * one_over_rho;
255  Lalpha[0][5] = -1.;
256 
257  L = vpMatrix::stack(L, Lalpha);
258  }
259 
260  return L;
261 }
262 
274 vpColVector vpFeatureVanishingPoint::error(const vpBasicFeature &s_star, unsigned int select)
275 {
276  vpColVector e(0);
277 
278  if (vpFeatureVanishingPoint::selectX() & select) {
279  vpColVector ex(1);
280  ex[0] = s[0] - s_star[0];
281 
282  e = vpColVector::stack(e, ex);
283  }
284 
285  if (vpFeatureVanishingPoint::selectY() & select) {
286  vpColVector ey(1);
287  ey[0] = s[1] - s_star[1];
288  e = vpColVector::stack(e, ey);
289  }
290 
292  vpColVector e_one_over_rho(1);
293  e_one_over_rho[0] = s[2] - s_star[2];
294 
295  e = vpColVector::stack(e, e_one_over_rho);
296  }
297 
299  vpColVector e_atan_one_over_rho(1);
300  e_atan_one_over_rho[0] = s[3] - s_star[3];
301 
302  e = vpColVector::stack(e, e_atan_one_over_rho);
303  }
304 
305  if (vpFeatureVanishingPoint::selectAlpha() & select) {
306  vpColVector e_alpha(1);
307  double err = s[4] - s_star[4];
308 
309  if (err < -M_PI)
310  err += 2 * M_PI;
311  if (err > M_PI)
312  err -= 2 * M_PI;
313 
314  e_alpha[0] = err;
315  e = vpColVector::stack(e, e_alpha);
316  }
317 
318  return e;
319 }
320 
329 void vpFeatureVanishingPoint::print(unsigned int select) const
330 {
331  std::cout << "Vanishing point:";
332  if (vpFeatureVanishingPoint::selectX() & select)
333  std::cout << " x=" << get_x();
334  if (vpFeatureVanishingPoint::selectY() & select)
335  std::cout << " y=" << get_y();
337  std::cout << " 1/rho=" << getOneOverRho();
338  }
340  std::cout << " atan(1/rho)=" << getAtanOneOverRho();
341  }
342  if (vpFeatureVanishingPoint::selectAlpha() & select) {
343  std::cout << " alpha=" << getAlpha();
344  }
345  std::cout << std::endl;
346 }
347 
348 
350 void vpFeatureVanishingPoint::buildFrom(double x, double y)
351 {
352  set_xy(x, y);
353 }
354 
364  const vpColor &color, unsigned int thickness) const
365 {
367  double x, y;
368  x = get_x();
369  y = get_y();
370 
371  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
372  }
374  double one_over_rho = getOneOverRho();
375  double alpha = getAlpha();
376  double x = cos(alpha) / one_over_rho;
377  double y = sin(alpha) / one_over_rho;
378  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
379  }
381  double atan_one_over_rho = getAtanOneOverRho();
382  double alpha = getAlpha();
383  double x = cos(alpha) / tan(atan_one_over_rho);
384  double y = sin(alpha) / tan(atan_one_over_rho);
385  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
386  }
387 }
388 
398  unsigned int thickness) const
399 {
401  double x, y;
402  x = get_x();
403  y = get_y();
404 
405  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
406  }
408  double one_over_rho = getOneOverRho();
409  double alpha = getAlpha();
410  double x = cos(alpha) / one_over_rho;
411  double y = sin(alpha) / one_over_rho;
412  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
413  }
415  double atan_one_over_rho = getAtanOneOverRho();
416  double alpha = getAlpha();
417  double x = cos(alpha) / tan(atan_one_over_rho);
418  double y = sin(alpha) / tan(atan_one_over_rho);
419  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
420  }
421 }
422 
427 {
429  return feature;
430 }
431 
433 unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0]; }
434 
436 unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1]; }
437 
442 
447 
452 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static unsigned int selectAtanOneOverRho()
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
double getAtanOneOverRho() const
Get vanishing point feature value.
unsigned int dim_s
Dimension of the visual feature.
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(selectX()|selectY()))
vpMatrix interaction(unsigned int select=(selectX()|selectY()))
void print(unsigned int select=(selectX()|selectY())) const
void set_xy(double x, double y)
Set vanishing point visual feature from cartesian coordinates. Same as buildFrom().
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
class that defines what is a visual feature
vpFeatureVanishingPoint * duplicate() const
static unsigned int selectX()
Select visual feature .
void buildFrom(double x, double y)
Set vanishing point visual feature from cartesian coordinates. Same as set_xy(). ...
#define vpTRACE
Definition: vpDebug.h:416
static unsigned int selectY()
Select visual feature .
Generic class defining intrinsic camera parameters.
void setAtanOneOverRho(double atan_one_over_rho)
Set vanishing point feature value.
vpFeatureVanishingPoint()
Default constructor that calls init().
double get_x() const
Get vanishing point feature value.
static unsigned int selectOneOverRho()
static const unsigned int FEATURE_LINE[32]
void setAlpha(double alpha)
Set vanishing point feature value.
double getAlpha() const
Get vanishing point feature value.
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
void set_y(double y)
Set vanishing point feature value.
static unsigned int selectAlpha()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_x(double x)
Set vanishing point feature value.
double getOneOverRho() const
Get vanishing point feature value.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void setOneOverRho(double one_over_rho)
Set vanishing point feature value.
double get_y() const
Get vanishing point feature value.