Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
vpFeatureBuilderLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 
103 {
104  try {
105  double a = t.getA();
106  double b = t.getB();
107  double c = t.getC();
108 
109  double x0 = t.getX();
110  double y0 = t.getY();
111  double z0 = t.getZ();
112 
113  double R = t.getR();
114 
115  double D =
116  vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0) - vpMath::sqr(R) - vpMath::sqr(a * x0 + b * y0 + c * z0);
117 
118  double alpha1 = (1 - a * a) * x0 - a * b * y0 - a * c * z0;
119  double beta1 = -a * b * x0 + (1 - b * b) * y0 - b * c * z0;
120  double gamma1 = -a * c * x0 - b * c * y0 + (1 - c * c) * z0;
121 
122  D *= -1;
123 
124  if (D < 0) {
125  alpha1 *= -1;
126  beta1 *= -1;
127  gamma1 *= -1;
128  D *= -1;
129  }
130 
131  s.setABCD(alpha1, beta1, gamma1, D);
132 
133  if (line == vpCylinder::line1) {
134 
135  s.setRhoTheta(t.getRho1(), t.getTheta1());
136 
137  } else {
138 
139  s.setRhoTheta(t.getRho2(), t.getTheta2());
140  }
141  } catch (...) {
142  vpERROR_TRACE("Error caught");
143  throw;
144  }
145 }
146 
147 #ifdef VISP_HAVE_MODULE_ME
195 {
196  try {
197  double rhop = t.getRho();
198  double thetap = t.getTheta();
199  double rho;
200  double theta;
201 
202  // Gives the rho and theta coordinates in the (u,v) coordinate system.
203  if (thetap >= 0 && thetap < M_PI / 2) {
204  thetap = M_PI / 2 - thetap;
205  }
206 
207  else if (thetap >= M_PI / 2 && thetap < 3 * M_PI / 2) {
208  thetap = 3 * M_PI / 2 + M_PI - thetap;
209  }
210 
211  else if (thetap >= 3 * M_PI / 2 && thetap <= 2 * M_PI) {
212  thetap = M_PI / 2 + 2 * M_PI - thetap;
213  }
214 
215  // while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
216  // while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
217 
218  // vpTRACE("pixel %f %f",rhop, thetap) ;
219  vpPixelMeterConversion::convertLine(cam, rhop, thetap, rho, theta);
220 
221  while (theta > M_PI) {
222  theta -= 2 * M_PI;
223  }
224  while (theta < -M_PI) {
225  theta += 2 * M_PI;
226  }
227  // vpTRACE("meter %f %f",rho, theta) ;
228  /*
229 
230  while(theta < -M_PI) theta += 2*M_PI ;
231  while(theta >= M_PI) theta -= 2*M_PI ;
232 
233  // If theta is between -90 and -180 get the equivalent
234  // between 0 and 90
235  if(theta <-M_PI/2)
236  {
237  theta += M_PI ;
238  rho *= -1 ;
239  }
240  // If theta is between 90 and 180 get the equivalent
241  // between 0 and -90
242  if(theta >M_PI/2)
243  {
244  theta -= M_PI ;
245  rho *= -1 ;
246  }
247  */
248  s.buildFrom(rho, theta);
249  // vpTRACE("meter %f %f",rho, theta) ;
250 
251  } catch (...) {
252  vpERROR_TRACE("Error caught");
253  throw;
254  }
255 }
256 #endif //#ifdef VISP_HAVE_MODULE_ME
Generic class defining intrinsic camera parameters.
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
double getZ() const
Definition: vpCylinder.h:180
double getRho1() const
Definition: vpCylinder.h:136
double getB() const
Definition: vpCylinder.h:164
double getX() const
Definition: vpCylinder.h:172
double getY() const
Definition: vpCylinder.h:176
double getA() const
Definition: vpCylinder.h:160
double getTheta1() const
Definition: vpCylinder.h:142
double getR() const
Definition: vpCylinder.h:184
double getC() const
Definition: vpCylinder.h:168
double getTheta2() const
Definition: vpCylinder.h:155
double getRho2() const
Definition: vpCylinder.h:149
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void setRhoTheta(double rho, double theta)
void buildFrom(double rho, double theta)
void setABCD(double A, double B, double C, double D)
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:105
double getRho() const
Definition: vpLine.h:134
double getTheta() const
Definition: vpLine.h:146
static double sqr(double x)
Definition: vpMath.h:123
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:152
double getRho() const
Definition: vpMeLine.cpp:955
double getTheta() const
Definition: vpMeLine.cpp:960
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)
vpColVector cP
Definition: vpTracker.h:77
#define vpERROR_TRACE
Definition: vpDebug.h:393