ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
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 }