Visual Servoing Platform version 3.5.0
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:116
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