Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpFeatureBuilderVanishingPoint.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 vanishing point.
33 *
34 * Authors:
35 * Odile Bourquardez
36 *
37*****************************************************************************/
38
44#include <visp3/core/vpException.h>
45#include <visp3/visual_features/vpFeatureBuilder.h>
46#include <visp3/visual_features/vpFeatureException.h>
47
64void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &p, unsigned int select)
65{
67 s.set_x(p.get_x());
68 s.set_y(p.get_y());
69 } else if ((vpFeatureVanishingPoint::selectOneOverRho() & select) ||
71 double x = p.get_x();
72 double y = p.get_y();
73
74 s.setOneOverRho(1. / sqrt(x * x + y * y));
75 s.setAlpha(atan2(y, x));
76 }
77}
78
100 unsigned int select)
101{
103 double rho_l = L1.getRho();
104 double rho_r = L2.getRho();
105 double theta_l = L1.getTheta();
106 double theta_r = L2.getTheta();
107 double c_l = cos(theta_l);
108 double c_r = cos(theta_r);
109 double s_l = sin(theta_l);
110 double s_r = sin(theta_r);
111
112 double min = 0.0001;
113 if (fabs(theta_r - theta_l) < min || fabs(fabs(theta_r - theta_l) - M_PI) < min ||
114 fabs(fabs(theta_r - theta_l) - 2 * M_PI) < min) {
115 vpCERROR << "There is no vanishing point : the lines are parallel in the "
116 "image plane"
117 << std::endl;
119 "There is no vanishing point : the lines are "
120 "parallel in the image plane"));
121 }
122
123 double y = (rho_r * c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l);
124 double x = (rho_r * s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l);
125
126 s.set_x(x);
127 s.set_y(y);
128 } else if ((vpFeatureVanishingPoint::selectOneOverRho() & select) ||
130 double rho_1 = L1.getRho();
131 double theta_1 = L1.getTheta();
132 double rho_2 = L2.getRho();
133 double theta_2 = L2.getTheta();
134
135 double theta_diff = theta_1 - theta_2;
136
137 double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
138 double one_over_rho = sin(theta_diff) / denom;
139 double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
140
141 s.setOneOverRho(one_over_rho);
142 s.setAlpha(alpha);
143 }
144}
145
167void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2, unsigned int select)
168{
169 vpFeatureLine l1, l2;
172
173 vpFeatureBuilder::create(s, l1, l2, select);
174}
175
194 const vpImagePoint &line1_ip2, const vpImagePoint &line2_ip1,
195 const vpImagePoint &line2_ip2, unsigned int select)
196{
197 double x1 = 0, y1 = 0;
198 double x2 = 0, y2 = 0;
199
200 // First line
201 vpPixelMeterConversion::convertPoint(cam, line1_ip1, x1, y1);
202 vpPixelMeterConversion::convertPoint(cam, line1_ip2, x2, y2);
203
204 double theta_1 = atan2(-(x1 - x2), y1 - y2);
205 double rho_1 = ((x1 + x2) * cos(theta_1) + (y1 + y2) * sin(theta_1)) / 2.;
206
207 // Second line
208 vpPixelMeterConversion::convertPoint(cam, line2_ip1, x1, y1);
209 vpPixelMeterConversion::convertPoint(cam, line2_ip2, x2, y2);
210
211 double theta_2 = atan2(-(x1 - x2), y1 - y2);
212 double rho_2 = ((x1 + x2) * cos(theta_2) + (y1 + y2) * sin(theta_2)) / 2.;
213
215 double min = 0.0001;
216 double theta_diff = theta_1 - theta_2;
217
218 if (fabs(theta_diff) < min || fabs(fabs(theta_diff) - M_PI) < min || fabs(fabs(theta_diff) - 2 * M_PI) < min) {
220 "There is no vanishing point : the lines are parallel in the image plane"));
221 }
222
223 double x = (sin(theta_1) * rho_2 - sin(theta_2) * rho_1) / sin(theta_diff);
224 double y = (cos(theta_2) * rho_1 - cos(theta_1) * rho_2) / sin(theta_diff);
225
226 s.set_x(x);
227 s.set_y(y);
228 } else if ((vpFeatureVanishingPoint::selectOneOverRho() & select)) {
229 double theta_diff = theta_1 - theta_2;
230 double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
231 double one_over_rho = sin(theta_diff) / denom;
232 // if (one_over_rho < 0) {
233 // one_over_rho = -one_over_rho;
234 // }
235 double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
236
237 s.setOneOverRho(one_over_rho);
238 s.setAlpha(alpha);
239 } else if ((vpFeatureVanishingPoint::selectAtanOneOverRho() & select)) {
240 double theta_diff = theta_1 - theta_2;
241 double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
242 double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
243 double one_over_rho = sin(theta_diff) / denom;
244 double atan_one_over_rho = atan2(sin(theta_diff), denom);
245
246 // if (one_over_rho < 0) {
247 // one_over_rho = -one_over_rho;
248 // }
249
250 s.setOneOverRho(one_over_rho);
251 s.setAtanOneOverRho(atan_one_over_rho);
252 s.setAlpha(alpha);
253 }
254}
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
double getTheta() const
double getRho() const
static unsigned int selectAtanOneOverRho()
void set_y(double y)
Set vanishing point feature value.
void setAlpha(double alpha)
Set vanishing point feature value.
void setAtanOneOverRho(double atan_one_over_rho)
Set vanishing point feature value.
void setOneOverRho(double one_over_rho)
Set vanishing point feature value.
static unsigned int selectX()
Select visual feature .
static unsigned int selectOneOverRho()
static unsigned int selectY()
Select visual feature .
void set_x(double x)
Set vanishing point feature value.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
#define vpCERROR
Definition vpDebug.h:360