Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 
59 {
60  // feature dimension
61  dim_s = 2;
62  nbParameters = 2;
63 
64  // memory allocation
65  s.resize(dim_s);
66  if (flags == NULL)
67  flags = new bool[nbParameters];
68  for (unsigned int i = 0; i < nbParameters; i++)
69  flags[i] = false;
70 
71  // Z not required (infinity)
72  // set_Z(1) ;
73 }
75 
77 void vpFeatureVanishingPoint::set_x(const double _x)
78 {
79  s[0] = _x;
80  flags[0] = true;
81 }
83 double vpFeatureVanishingPoint::get_x() const { return s[0]; }
84 
86 void vpFeatureVanishingPoint::set_y(const double _y)
87 {
88  s[1] = _y;
89  flags[1] = true;
90 }
92 double vpFeatureVanishingPoint::get_y() const { return s[1]; }
93 
95 void vpFeatureVanishingPoint::set_xy(const double _x, const double _y)
96 {
97  set_x(_x);
98  set_y(_y);
99 }
100 
103 {
104  vpMatrix L;
105 
106  L.resize(0, 6);
107 
109  for (unsigned int i = 0; i < nbParameters; i++) {
110  if (flags[i] == false) {
111  switch (i) {
112  case 0:
113  vpTRACE("Warning !!! The interaction matrix is computed but x was "
114  "not set yet");
115  break;
116  case 1:
117  vpTRACE("Warning !!! The interaction matrix is computed but y was "
118  "not set yet");
119  break;
120  default:
121  vpTRACE("Problem during the reading of the variable flags");
122  }
123  }
124  }
125  resetFlags();
126  }
127 
128  double x = get_x();
129  double y = get_y();
130 
131  if (vpFeatureVanishingPoint::selectX() & select) {
132  vpMatrix Lx(1, 6);
133  Lx = 0;
134 
135  Lx[0][0] = 0.;
136  Lx[0][1] = 0.;
137  Lx[0][2] = 0.;
138  Lx[0][3] = x * y;
139  Lx[0][4] = -(1 + x * x);
140  Lx[0][5] = y;
141 
142  L = vpMatrix::stack(L, Lx);
143  }
144 
145  if (vpFeatureVanishingPoint::selectY() & select) {
146  vpMatrix Ly(1, 6);
147  Ly = 0;
148 
149  Ly[0][0] = 0;
150  Ly[0][1] = 0.;
151  Ly[0][2] = 0.;
152  Ly[0][3] = 1 + y * y;
153  Ly[0][4] = -x * y;
154  Ly[0][5] = -x;
155 
156  L = vpMatrix::stack(L, Ly);
157  }
158  return L;
159 }
160 
164 vpColVector vpFeatureVanishingPoint::error(const vpBasicFeature &s_star, const unsigned int select)
165 {
166  vpColVector e(0);
167 
168  try {
169  if (vpFeatureVanishingPoint::selectX() & select) {
170  vpColVector ex(1);
171  ex[0] = s[0] - s_star[0];
172 
173  e = vpColVector::stack(e, ex);
174  }
175 
176  if (vpFeatureVanishingPoint::selectY() & select) {
177  vpColVector ey(1);
178  ey[0] = s[1] - s_star[1];
179  e = vpColVector::stack(e, ey);
180  }
181  } catch (...) {
182  throw;
183  }
184  return e;
185 }
186 
187 void vpFeatureVanishingPoint::print(const unsigned int select) const
188 {
189 
190  std::cout << "Point: " << std::endl;
191  if (vpFeatureVanishingPoint::selectX() & select)
192  std::cout << " x=" << get_x();
193  if (vpFeatureVanishingPoint::selectY() & select)
194  std::cout << " y=" << get_y();
195  std::cout << std::endl;
196 }
197 
198 void vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
199 {
200  s[0] = _x;
201  s[1] = _y;
202  for (unsigned int i = 0; i < nbParameters; i++)
203  flags[i] = true;
204 }
205 
217  const vpColor &color, unsigned int thickness) const
218 {
219  try {
220  double x, y;
221  x = get_x();
222  y = get_y();
223 
224  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
225 
226  } catch (...) {
227  vpERROR_TRACE("Error caught");
228  throw;
229  }
230 }
242  unsigned int thickness) const
243 {
244  try {
245  double x, y;
246  x = get_x();
247  y = get_y();
248 
249  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
250 
251  } catch (...) {
252  vpERROR_TRACE("Error caught");
253  throw;
254  }
255 }
256 
260 {
262  return feature;
263 }
264 
265 unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0]; }
266 unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void buildFrom(const double _x, const double _y)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4551
unsigned int dim_s
Dimension of the visual feature.
void set_xy(const double _x, const double _y)
Set the point xy coordinates.
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
#define vpTRACE
Definition: vpDebug.h:416
double get_y() const
get the point y-coordinates
Generic class defining intrinsic camera parameters.
void print(const unsigned int select=FEATURE_ALL) const
print the name of the feature
vpFeatureVanishingPoint()
Default constructor.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
vpFeatureVanishingPoint * duplicate() const
feature duplication
static const unsigned int FEATURE_LINE[32]
void set_x(const double _x)
Set the point x-coordinates.
vpBasicFeatureDeallocatorType deallocate
void set_y(const double _y)
Set the point y-coordinates.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
double get_x() const
get the point x-coordinates
void init()
Default initialization.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310