Visual Servoing Platform  version 3.1.0
vpFeatureBuilderLine.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 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  * Conversion between tracker and visual feature line.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpMath.h>
46 #include <visp3/visual_features/vpFeatureBuilder.h>
47 
61 {
62  try {
63  double A, B, C, D;
64  s.setRhoTheta(t.getRho(), t.getTheta());
65 
66  if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
67  {
68  A = t.cP[0];
69  B = t.cP[1];
70  C = t.cP[2];
71  D = t.cP[3];
72  } else {
73  A = t.cP[4];
74  B = t.cP[5];
75  C = t.cP[6];
76  D = t.cP[7];
77  }
78 
79  s.setABCD(A, B, C, D);
80 
81  } catch (...) {
82  vpERROR_TRACE("Error caught");
83  throw;
84  }
85 }
86 
102 void vpFeatureBuilder::create(vpFeatureLine &s, const vpCylinder &t, const int line)
103 {
104  try {
105 
106  double a = t.getA();
107  double b = t.getB();
108  double c = t.getC();
109 
110  double x0 = t.getX();
111  double y0 = t.getY();
112  double z0 = t.getZ();
113 
114  double R = t.getR();
115 
116  double D =
117  vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0) - vpMath::sqr(R) - vpMath::sqr(a * x0 + b * y0 + c * z0);
118 
119  double alpha1 = (1 - a * a) * x0 - a * b * y0 - a * c * z0;
120  double beta1 = -a * b * x0 + (1 - b * b) * y0 - b * c * z0;
121  double gamma1 = -a * c * x0 - b * c * y0 + (1 - c * c) * z0;
122 
123  D *= -1;
124 
125  if (D < 0) {
126  alpha1 *= -1;
127  beta1 *= -1;
128  gamma1 *= -1;
129  D *= -1;
130  }
131 
132  s.setABCD(alpha1, beta1, gamma1, D);
133 
134  if (line == vpCylinder::line1) {
135 
136  s.setRhoTheta(t.getRho1(), t.getTheta1());
137 
138  } else {
139 
140  s.setRhoTheta(t.getRho2(), t.getTheta2());
141  }
142  } catch (...) {
143  vpERROR_TRACE("Error caught");
144  throw;
145  }
146 }
147 
148 #ifdef VISP_HAVE_MODULE_ME
149 
196 {
197  try {
198  double rhop = t.getRho();
199  double thetap = t.getTheta();
200  double rho;
201  double theta;
202 
203  // Gives the rho and theta coordinates in the (u,v) coordinate system.
204  if (thetap >= 0 && thetap < M_PI / 2) {
205  thetap = M_PI / 2 - thetap;
206  }
207 
208  else if (thetap >= M_PI / 2 && thetap < 3 * M_PI / 2) {
209  thetap = 3 * M_PI / 2 + M_PI - thetap;
210  }
211 
212  else if (thetap >= 3 * M_PI / 2 && thetap <= 2 * M_PI) {
213  thetap = M_PI / 2 + 2 * M_PI - thetap;
214  }
215 
216  // while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
217  // while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
218 
219  // vpTRACE("pixel %f %f",rhop, thetap) ;
220  vpPixelMeterConversion::convertLine(cam, rhop, thetap, rho, theta);
221 
222  while (theta > M_PI) {
223  theta -= 2 * M_PI;
224  }
225  while (theta < -M_PI) {
226  theta += 2 * M_PI;
227  }
228  // vpTRACE("meter %f %f",rho, theta) ;
229  /*
230 
231  while(theta < -M_PI) theta += 2*M_PI ;
232  while(theta >= M_PI) theta -= 2*M_PI ;
233 
234  // If theta is between -90 and -180 get the equivalent
235  // between 0 and 90
236  if(theta <-M_PI/2)
237  {
238  theta += M_PI ;
239  rho *= -1 ;
240  }
241  // If theta is between 90 and 180 get the equivalent
242  // between 0 and -90
243  if(theta >M_PI/2)
244  {
245  theta -= M_PI ;
246  rho *= -1 ;
247  }
248  */
249  s.buildFrom(rho, theta);
250  // vpTRACE("meter %f %f",rho, theta) ;
251 
252  } catch (...) {
253  vpERROR_TRACE("Error caught");
254  throw;
255  }
256 }
257 #endif //#ifdef VISP_HAVE_MODULE_ME
void setABCD(const double A, const double B, const double C, const double D)
double getTheta() const
Definition: vpMeLine.cpp:959
double getR() const
Definition: vpCylinder.h:175
double getA() const
Definition: vpCylinder.h:151
#define vpERROR_TRACE
Definition: vpDebug.h:393
double getB() const
Definition: vpCylinder.h:155
double getRho() const
Definition: vpMeLine.cpp:954
double getC() const
Definition: vpCylinder.h:159
vpColVector cP
Definition: vpTracker.h:75
void buildFrom(const double rho, const double theta)
double getTheta2() const
Definition: vpCylinder.h:146
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:105
static double sqr(double x)
Definition: vpMath.h:108
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:151
Generic class defining intrinsic camera parameters.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
double getY() const
Definition: vpCylinder.h:167
Class that defines what is a cylinder.
Definition: vpCylinder.h:96
double getTheta() const
Definition: vpLine.h:146
double getZ() const
Definition: vpCylinder.h:171
double getRho1() const
Definition: vpCylinder.h:127
double getRho() const
Definition: vpLine.h:157
double getX() const
Definition: vpCylinder.h:163
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 getTheta1() const
Definition: vpCylinder.h:133
double getRho2() const
Definition: vpCylinder.h:140
void setRhoTheta(const double rho, const double theta)