Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpFeatureBuilderLine.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
42 #include <visp3/core/vpMath.h>
43 #include <visp3/visual_features/vpFeatureBuilder.h>
44 
58 {
59  try {
60  double A, B, C, D;
61  s.setRhoTheta(t.getRho(), t.getTheta());
62 
63  if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
64  {
65  A = t.cP[0];
66  B = t.cP[1];
67  C = t.cP[2];
68  D = t.cP[3];
69  } else {
70  A = t.cP[4];
71  B = t.cP[5];
72  C = t.cP[6];
73  D = t.cP[7];
74  }
75 
76  s.setABCD(A, B, C, D);
77 
78  } catch (...) {
79  vpERROR_TRACE("Error caught");
80  throw;
81  }
82 }
83 
100 {
101  try {
102  double a = t.getA();
103  double b = t.getB();
104  double c = t.getC();
105 
106  double x0 = t.getX();
107  double y0 = t.getY();
108  double z0 = t.getZ();
109 
110  double R = t.getR();
111 
112  double D =
113  vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0) - vpMath::sqr(R) - vpMath::sqr(a * x0 + b * y0 + c * z0);
114 
115  double alpha1 = (1 - a * a) * x0 - a * b * y0 - a * c * z0;
116  double beta1 = -a * b * x0 + (1 - b * b) * y0 - b * c * z0;
117  double gamma1 = -a * c * x0 - b * c * y0 + (1 - c * c) * z0;
118 
119  D *= -1;
120 
121  if (D < 0) {
122  alpha1 *= -1;
123  beta1 *= -1;
124  gamma1 *= -1;
125  D *= -1;
126  }
127 
128  s.setABCD(alpha1, beta1, gamma1, D);
129 
130  if (line == vpCylinder::line1) {
131 
132  s.setRhoTheta(t.getRho1(), t.getTheta1());
133 
134  } else {
135 
136  s.setRhoTheta(t.getRho2(), t.getTheta2());
137  }
138  } catch (...) {
139  vpERROR_TRACE("Error caught");
140  throw;
141  }
142 }
143 
144 #ifdef VISP_HAVE_MODULE_ME
192 {
193  try {
194  double rhop = t.getRho();
195  double thetap = t.getTheta();
196  double rho;
197  double theta;
198 
199  // Gives the rho and theta coordinates in the (u,v) coordinate system.
200  if (thetap >= 0 && thetap < M_PI / 2) {
201  thetap = M_PI / 2 - thetap;
202  }
203 
204  else if (thetap >= M_PI / 2 && thetap < 3 * M_PI / 2) {
205  thetap = 3 * M_PI / 2 + M_PI - thetap;
206  }
207 
208  else if (thetap >= 3 * M_PI / 2 && thetap <= 2 * M_PI) {
209  thetap = M_PI / 2 + 2 * M_PI - thetap;
210  }
211 
212  // while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
213  // while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
214 
215  // vpTRACE("pixel %f %f",rhop, thetap) ;
216  vpPixelMeterConversion::convertLine(cam, rhop, thetap, rho, theta);
217 
218  while (theta > M_PI) {
219  theta -= 2 * M_PI;
220  }
221  while (theta < -M_PI) {
222  theta += 2 * M_PI;
223  }
224 
225  s.buildFrom(rho, theta);
226  } catch (...) {
227  vpERROR_TRACE("Error caught");
228  throw;
229  }
230 }
231 #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:99
double getZ() const
Definition: vpCylinder.h:181
double getRho1() const
Definition: vpCylinder.h:132
double getB() const
Definition: vpCylinder.h:161
double getX() const
Definition: vpCylinder.h:171
double getY() const
Definition: vpCylinder.h:176
double getA() const
Definition: vpCylinder.h:156
double getTheta1() const
Definition: vpCylinder.h:138
double getR() const
Definition: vpCylinder.h:186
double getC() const
Definition: vpCylinder.h:166
double getTheta2() const
Definition: vpCylinder.h:151
double getRho2() const
Definition: vpCylinder.h:145
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:101
double getRho() const
Definition: vpLine.h:130
double getTheta() const
Definition: vpLine.h:141
static double sqr(double x)
Definition: vpMath.h:201
Class that tracks in an image a line moving edges.
Definition: vpMeLine.h:147
double getRho() const
Definition: vpMeLine.cpp:832
double getTheta() const
Definition: vpMeLine.cpp:834
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:71
#define vpERROR_TRACE
Definition: vpDebug.h:382