ViSP  2.9.0
vpFeatureBuilderLine.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureBuilderLine.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  * Conversion between tracker and visual feature line.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpFeatureBuilder.h>
50 #include <visp/vpMath.h>
51 
52 
53 
65 {
66  try
67  {
68  double A,B,C,D ;
69  s.setRhoTheta(t.getRho(),t.getTheta()) ;
70 
71  if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
72  {
73  A = t.cP[0] ;
74  B = t.cP[1] ;
75  C = t.cP[2] ;
76  D = t.cP[3] ;
77  }
78  else
79  {
80  A = t.cP[4] ;
81  B = t.cP[5] ;
82  C = t.cP[6] ;
83  D = t.cP[7] ;
84  }
85 
86 
87  s.setABCD(A,B,C,D) ;
88 
89  }
90  catch(...)
91  {
92  vpERROR_TRACE("Error caught") ;
93  throw ;
94  }
95 
96 }
97 
113  const vpCylinder &t,
114  const int line)
115 {
116  try
117  {
118 
119  double a = t.getA() ;
120  double b = t.getB() ;
121  double c = t.getC() ;
122 
123  double x0 = t.getX() ;
124  double y0 = t.getY() ;
125  double z0 = t.getZ() ;
126 
127  double R = t.getR() ;
128 
129  double D =
130  vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0)
131  - vpMath::sqr(R)
132  - vpMath::sqr(a*x0 + b*y0 + c*z0);
133 
134  double alpha1 = (1 - a*a)*x0 - a*b*y0 - a*c*z0;
135  double beta1 = -a*b*x0 + (1 - b*b)*y0 - b*c*z0;
136  double gamma1 = -a*c*x0 - b*c*y0 + (1 - c*c)*z0;
137 
138  D*=-1 ;
139 
140  if (D<0)
141  {
142  alpha1*=-1 ;
143  beta1*=-1 ;
144  gamma1*=-1 ;
145  D*=-1 ;
146  }
147 
148  s.setABCD(alpha1,beta1,gamma1,D) ;
149 
150  if (line==vpCylinder::line1)
151  {
152 
153  s.setRhoTheta(t.getRho1(),t.getTheta1()) ;
154 
155  }
156  else
157  {
158 
159  s.setRhoTheta(t.getRho2(),t.getTheta2()) ;
160  }
161  }
162  catch(...)
163  {
164  vpERROR_TRACE("Error caught") ;
165  throw ;
166  }
167 }
168 
169 
213 void
215  const vpCameraParameters &cam,
216  const vpMeLine &t)
217 {
218  double rhop ;
219  double thetap ;
220 
221  double rho ;
222  double theta ;
223 
224  try{
225  rhop = t.getRho() ;
226  thetap = t.getTheta();
227 
228  //Gives the rho and theta coordinates in the (u,v) coordinate system.
229  if (thetap >= 0 && thetap < M_PI/2)
230  {
231  thetap = M_PI/2 - thetap;
232  }
233 
234  else if (thetap >= M_PI/2 && thetap < 3*M_PI/2)
235  {
236  thetap = 3*M_PI/2 + M_PI - thetap;
237  }
238 
239  else if (thetap >= 3*M_PI/2 && thetap <= 2*M_PI)
240  {
241  thetap = M_PI/2 + 2*M_PI - thetap;
242  }
243 
244  //while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
245  //while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
246 
247  // vpTRACE("pixel %f %f",rhop, thetap) ;
248  vpPixelMeterConversion::convertLine(cam,rhop,thetap, rho,theta) ;
249 
250  while (theta > M_PI) { theta -= 2*M_PI ; }
251  while (theta < -M_PI) { theta += 2*M_PI ; }
252  // vpTRACE("meter %f %f",rho, theta) ;
253  /*
254 
255  while(theta < -M_PI) theta += 2*M_PI ;
256  while(theta >= M_PI) theta -= 2*M_PI ;
257 
258  // If theta is between -90 and -180 get the equivalent
259  // between 0 and 90
260  if(theta <-M_PI/2)
261  {
262  theta += M_PI ;
263  rho *= -1 ;
264  }
265  // If theta is between 90 and 180 get the equivalent
266  // between 0 and -90
267  if(theta >M_PI/2)
268  {
269  theta -= M_PI ;
270  rho *= -1 ;
271  }
272  */
273  s.buildFrom(rho,theta) ;
274  // vpTRACE("meter %f %f",rho, theta) ;
275 
276  }
277  catch(...)
278  {
279  vpERROR_TRACE("Error caught") ;
280  throw ;
281  }
282 }
283 
284 /*
285  * Local variables:
286  * c-basic-offset: 2
287  * End:
288  */
void setABCD(const double A, const double B, const double C, const double D)
double getY() const
Definition: vpCylinder.h:175
#define vpERROR_TRACE
Definition: vpDebug.h:395
double getTheta() const
Definition: vpMeLine.cpp:1061
double getZ() const
Definition: vpCylinder.h:179
double getRho2() const
Definition: vpCylinder.h:148
double getTheta() const
Definition: vpLine.h:166
vpColVector cP
Definition: vpTracker.h:82
double getRho() const
Definition: vpLine.h:177
void buildFrom(const double rho, const double theta)
double getTheta1() const
Definition: vpCylinder.h:141
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:124
double getC() const
Definition: vpCylinder.h:167
static double sqr(double x)
Definition: vpMath.h:106
double getB() const
Definition: vpCylinder.h:163
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:156
Generic class defining intrinsic camera parameters.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
double getA() const
Definition: vpCylinder.h:159
double getRho1() const
Definition: vpCylinder.h:135
double getTheta2() const
Definition: vpCylinder.h:154
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)
line coordinates conversion (rho,theta)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double getRho() const
Definition: vpMeLine.cpp:1052
double getX() const
Definition: vpCylinder.h:171
double getR() const
Definition: vpCylinder.h:183
void setRhoTheta(const double rho, const double theta)