Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureVanishingPoint.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
32  *
33  * Authors:
34  * Odile Bourquardez
35  *
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 void
60 {
61  //feature dimension
62  dim_s = 2 ;
63  nbParameters = 2;
64 
65  // memory allocation
66  s.resize(dim_s) ;
67  if (flags == NULL)
68  flags = new bool[nbParameters];
69  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
70 
71  //Z not required (infinity)
72  //set_Z(1) ;
73 
74 }
76 {
77  init() ;
78 }
79 
80 
82 void
84 {
85  s[0] = _x ;
86  flags[0] = true;
87 }
89 double
91 {
92  return s[0] ;
93 }
94 
96 void
98 {
99  s[1] = _y ;
100  flags[1] = true;
101 }
103 double
105 {
106  return s[1] ;
107 }
108 
109 
111 void
113  const double _y)
114 {
115  set_x(_x) ;
116  set_y(_y) ;
117 }
118 
119 
121 vpMatrix
122 vpFeatureVanishingPoint::interaction(const unsigned int select)
123 {
124  vpMatrix L ;
125 
126  L.resize(0,6) ;
127 
129  {
130  for (unsigned int i = 0; i < nbParameters; i++)
131  {
132  if (flags[i] == false)
133  {
134  switch(i){
135  case 0:
136  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
137  break;
138  case 1:
139  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
140  break;
141  default:
142  vpTRACE("Problem during the reading of the variable flags");
143  }
144  }
145  }
146  resetFlags();
147  }
148 
149  double x = get_x() ;
150  double y = get_y() ;
151 
152  if (vpFeatureVanishingPoint::selectX() & select )
153  {
154  vpMatrix Lx(1,6) ; Lx = 0;
155 
156  Lx[0][0] = 0. ;
157  Lx[0][1] = 0. ;
158  Lx[0][2] = 0. ;
159  Lx[0][3] = x*y ;
160  Lx[0][4] = -(1+x*x) ;
161  Lx[0][5] = y ;
162 
163  L = vpMatrix::stack(L,Lx) ;
164  }
165 
166  if (vpFeatureVanishingPoint::selectY() & select )
167  {
168  vpMatrix Ly(1,6) ; Ly = 0;
169 
170  Ly[0][0] = 0 ;
171  Ly[0][1] = 0. ;
172  Ly[0][2] = 0. ;
173  Ly[0][3] = 1+y*y ;
174  Ly[0][4] = -x*y ;
175  Ly[0][5] = -x ;
176 
177  L = vpMatrix::stack(L,Ly) ;
178  }
179  return L ;
180 }
181 
182 
188  const unsigned int select)
189 {
190  vpColVector e(0) ;
191 
192  try{
193  if (vpFeatureVanishingPoint::selectX() & select )
194  {
195  vpColVector ex(1) ;
196  ex[0] = s[0] - s_star[0] ;
197 
198  e = vpColVector::stack(e,ex) ;
199  }
200 
201  if (vpFeatureVanishingPoint::selectY() & select )
202  {
203  vpColVector ey(1) ;
204  ey[0] = s[1] - s_star[1] ;
205  e = vpColVector::stack(e,ey) ;
206  }
207  }
208  catch(...) {
209  throw ;
210  }
211  return e ;
212 }
213 
214 
215 
216 void
217 vpFeatureVanishingPoint::print(const unsigned int select ) const
218 {
219 
220  std::cout <<"Point: " <<std::endl;
221  if (vpFeatureVanishingPoint::selectX() & select )
222  std::cout << " x=" << get_x() ;
223  if (vpFeatureVanishingPoint::selectY() & select )
224  std::cout << " y=" << get_y() ;
225  std::cout <<std::endl ;
226 }
227 
228 
229 void
230 vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
231 {
232  s[0] = _x ;
233  s[1] = _y ;
234  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
235 }
236 
237 
248 void
250  const vpImage<unsigned char> &I,
251  const vpColor &color,
252  unsigned int thickness) const
253 {
254  try{
255  double x,y ;
256  x = get_x() ;
257  y = get_y() ;
258 
259  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
260 
261  }
262  catch(...)
263  {
264  vpERROR_TRACE("Error caught") ;
265  throw ;
266  }
267 }
278 void
280  const vpImage<vpRGBa> &I,
281  const vpColor &color,
282  unsigned int thickness) const
283 {
284  try{
285  double x,y ;
286  x = get_x() ;
287  y = get_y() ;
288 
289  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
290 
291  }
292  catch(...)
293  {
294  vpERROR_TRACE("Error caught") ;
295  throw ;
296  }
297 }
298 
299 
303 {
305  return feature ;
306 }
307 
308 unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0] ; }
309 unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void buildFrom(const double _x, const double _y)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
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:414
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:72
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:225