Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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
148 
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
void setABCD(double A, double B, double C, double D)
double getTheta() const
Definition: vpMeLine.cpp:960
double getR() const
Definition: vpCylinder.h:184
double getA() const
Definition: vpCylinder.h:160
#define vpERROR_TRACE
Definition: vpDebug.h:393
double getB() const
Definition: vpCylinder.h:164
double getRho() const
Definition: vpMeLine.cpp:955
double getC() const
Definition: vpCylinder.h:168
void buildFrom(double rho, double theta)
vpColVector cP
Definition: vpTracker.h:77
double getTheta2() const
Definition: vpCylinder.h:155
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:104
static double sqr(double x)
Definition: vpMath.h:116
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:176
void setRhoTheta(double rho, double theta)
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:102
double getTheta() const
Definition: vpLine.h:146
double getZ() const
Definition: vpCylinder.h:180
double getRho1() const
Definition: vpCylinder.h:136
double getRho() const
Definition: vpLine.h:134
double getX() const
Definition: vpCylinder.h:172
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
double getTheta1() const
Definition: vpCylinder.h:142
double getRho2() const
Definition: vpCylinder.h:149