ViSP  2.8.0
vpFeatureVanishingPoint.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureVanishingPoint.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * 2D vanishing point visual feature (Z coordinate in 3D space is infinity)
36  *
37  * Authors:
38  * Odile Bourquardez
39  *
40  *****************************************************************************/
41 
42 
47 #include <visp/vpBasicFeature.h>
48 #include <visp/vpFeatureVanishingPoint.h>
49 
50 // Exception
51 #include <visp/vpException.h>
52 #include <visp/vpMatrixException.h>
53 #include <visp/vpFeatureException.h>
54 
55 // Debug trace
56 #include <visp/vpDebug.h>
57 
58 // math
59 #include <visp/vpMath.h>
60 
61 #include <visp/vpFeatureDisplay.h>
62 
63 void
65 {
66  //feature dimension
67  dim_s = 2 ;
68  nbParameters = 2;
69 
70  // memory allocation
71  s.resize(dim_s) ;
72  if (flags == NULL)
73  flags = new bool[nbParameters];
74  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
75 
76  //Z not required (infinity)
77  //set_Z(1) ;
78 
79 }
81 {
82  init() ;
83 }
84 
85 
87 void
89 {
90  s[0] = _x ;
91  flags[0] = true;
92 }
94 double
96 {
97  return s[0] ;
98 }
99 
101 void
103 {
104  s[1] = _y ;
105  flags[1] = true;
106 }
108 double
110 {
111  return s[1] ;
112 }
113 
114 
116 void
118  const double _y)
119 {
120  set_x(_x) ;
121  set_y(_y) ;
122 }
123 
124 
126 vpMatrix
127 vpFeatureVanishingPoint::interaction(const unsigned int select)
128 {
129  vpMatrix L ;
130 
131  L.resize(0,6) ;
132 
134  {
135  for (unsigned int i = 0; i < nbParameters; i++)
136  {
137  if (flags[i] == false)
138  {
139  switch(i){
140  case 0:
141  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
142  break;
143  case 1:
144  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
145  break;
146  default:
147  vpTRACE("Problem during the reading of the variable flags");
148  }
149  }
150  }
151  resetFlags();
152  }
153 
154  double x = get_x() ;
155  double y = get_y() ;
156 
157  if (vpFeatureVanishingPoint::selectX() & select )
158  {
159  vpMatrix Lx(1,6) ; Lx = 0;
160 
161  Lx[0][0] = 0. ;
162  Lx[0][1] = 0. ;
163  Lx[0][2] = 0. ;
164  Lx[0][3] = x*y ;
165  Lx[0][4] = -(1+x*x) ;
166  Lx[0][5] = y ;
167 
168  L = vpMatrix::stackMatrices(L,Lx) ;
169  }
170 
171  if (vpFeatureVanishingPoint::selectY() & select )
172  {
173  vpMatrix Ly(1,6) ; Ly = 0;
174 
175  Ly[0][0] = 0 ;
176  Ly[0][1] = 0. ;
177  Ly[0][2] = 0. ;
178  Ly[0][3] = 1+y*y ;
179  Ly[0][4] = -x*y ;
180  Ly[0][5] = -x ;
181 
182  L = vpMatrix::stackMatrices(L,Ly) ;
183  }
184  return L ;
185 }
186 
187 
193  const unsigned int select)
194 {
195  vpColVector e(0) ;
196 
197  try{
198  if (vpFeatureVanishingPoint::selectX() & select )
199  {
200  vpColVector ex(1) ;
201  ex[0] = s[0] - s_star[0] ;
202 
203  e = vpMatrix::stackMatrices(e,ex) ;
204  }
205 
206  if (vpFeatureVanishingPoint::selectY() & select )
207  {
208  vpColVector ey(1) ;
209  ey[0] = s[1] - s_star[1] ;
210  e = vpMatrix::stackMatrices(e,ey) ;
211  }
212  }
213  catch(vpMatrixException me)
214  {
215  vpERROR_TRACE("caught a Matrix related error") ;
216  std::cout <<std::endl << me << std::endl ;
217  throw(me) ;
218  }
219  catch(vpException me)
220  {
221  vpERROR_TRACE("caught another error") ;
222  std::cout <<std::endl << me << std::endl ;
223  throw(me) ;
224  }
225  return e ;
226 }
227 
228 
229 
230 void
231 vpFeatureVanishingPoint::print(const unsigned int select ) const
232 {
233 
234  std::cout <<"Point: " <<std::endl;
235  if (vpFeatureVanishingPoint::selectX() & select )
236  std::cout << " x=" << get_x() ;
237  if (vpFeatureVanishingPoint::selectY() & select )
238  std::cout << " y=" << get_y() ;
239  std::cout <<std::endl ;
240 }
241 
242 
243 void
244 vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
245 {
246  s[0] = _x ;
247  s[1] = _y ;
248  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
249 }
250 
251 
262 void
264  const vpImage<unsigned char> &I,
265  const vpColor &color,
266  unsigned int thickness) const
267 {
268  try{
269  double x,y ;
270  x = get_x() ;
271  y = get_y() ;
272 
273  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
274 
275  }
276  catch(...)
277  {
278  vpERROR_TRACE("Error caught") ;
279  throw ;
280  }
281 }
292 void
294  const vpImage<vpRGBa> &I,
295  const vpColor &color,
296  unsigned int thickness) const
297 {
298  try{
299  double x,y ;
300  x = get_x() ;
301  y = get_y() ;
302 
303  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
304 
305  }
306  catch(...)
307  {
308  vpERROR_TRACE("Error caught") ;
309  throw ;
310  }
311 }
312 
313 
317 {
319  return feature ;
320 }
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
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:379
#define vpTRACE
Definition: vpDebug.h:401
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:75
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
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
static unsigned int selectY()
vpFeatureVanishingPoint()
basic 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 vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
void set_x(const double _x)
set the point x-coordinates
vpBasicFeatureDeallocatorType deallocate
void set_y(const double _y)
set the point y-coordinates
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double get_x() const
get the point x-coordinates
static unsigned int selectX()
error that can be emited by the vpMatrix class and its derivates
void init()
basic construction
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:94