Visual Servoing Platform  version 3.6.1 under development (2024-11-05)
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 
48 BEGIN_VISP_NAMESPACE
65 void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &p, unsigned int select)
66 {
67  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
68  s.set_x(p.get_x());
69  s.set_y(p.get_y());
70  }
71  else if ((vpFeatureVanishingPoint::selectOneOverRho() & select) ||
73  double x = p.get_x();
74  double y = p.get_y();
75 
76  s.setOneOverRho(1. / sqrt(x * x + y * y));
77  s.setAlpha(atan2(y, x));
78  }
79 }
80 
102  unsigned int select)
103 {
104  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
105  double rho_l = L1.getRho();
106  double rho_r = L2.getRho();
107  double theta_l = L1.getTheta();
108  double theta_r = L2.getTheta();
109  double c_l = cos(theta_l);
110  double c_r = cos(theta_r);
111  double s_l = sin(theta_l);
112  double s_r = sin(theta_r);
113 
114  double min = 0.0001;
115  if (fabs(theta_r - theta_l) < min || fabs(fabs(theta_r - theta_l) - M_PI) < min ||
116  fabs(fabs(theta_r - theta_l) - 2 * M_PI) < min) {
117  vpCERROR << "There is no vanishing point : the lines are parallel in the "
118  "image plane"
119  << std::endl;
121  "There is no vanishing point : the lines are "
122  "parallel in the image plane"));
123  }
124 
125  double y = (rho_r * c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l);
126  double x = (rho_r * s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l);
127 
128  s.set_x(x);
129  s.set_y(y);
130  }
131  else if ((vpFeatureVanishingPoint::selectOneOverRho() & select) ||
133  double rho_1 = L1.getRho();
134  double theta_1 = L1.getTheta();
135  double rho_2 = L2.getRho();
136  double theta_2 = L2.getTheta();
137 
138  double theta_diff = theta_1 - theta_2;
139 
140  double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
141  double one_over_rho = sin(theta_diff) / denom;
142  double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
143 
144  s.setOneOverRho(one_over_rho);
145  s.setAlpha(alpha);
146  }
147 }
148 
170 void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpLine &L1, const vpLine &L2, unsigned int select)
171 {
172  vpFeatureLine l1, l2;
173  vpFeatureBuilder::create(l1, L1);
174  vpFeatureBuilder::create(l2, L2);
175 
176  vpFeatureBuilder::create(s, l1, l2, select);
177 }
178 
197  const vpImagePoint &line1_ip2, const vpImagePoint &line2_ip1,
198  const vpImagePoint &line2_ip2, unsigned int select)
199 {
200  double x1 = 0, y1 = 0;
201  double x2 = 0, y2 = 0;
202 
203  // First line
204  vpPixelMeterConversion::convertPoint(cam, line1_ip1, x1, y1);
205  vpPixelMeterConversion::convertPoint(cam, line1_ip2, x2, y2);
206 
207  double theta_1 = atan2(-(x1 - x2), y1 - y2);
208  double rho_1 = ((x1 + x2) * cos(theta_1) + (y1 + y2) * sin(theta_1)) / 2.;
209 
210  // Second line
211  vpPixelMeterConversion::convertPoint(cam, line2_ip1, x1, y1);
212  vpPixelMeterConversion::convertPoint(cam, line2_ip2, x2, y2);
213 
214  double theta_2 = atan2(-(x1 - x2), y1 - y2);
215  double rho_2 = ((x1 + x2) * cos(theta_2) + (y1 + y2) * sin(theta_2)) / 2.;
216 
217  if ((vpFeatureVanishingPoint::selectX() & select) || (vpFeatureVanishingPoint::selectY() & select)) {
218  double min = 0.0001;
219  double theta_diff = theta_1 - theta_2;
220 
221  if (fabs(theta_diff) < min || fabs(fabs(theta_diff) - M_PI) < min || fabs(fabs(theta_diff) - 2 * M_PI) < min) {
223  "There is no vanishing point : the lines are parallel in the image plane"));
224  }
225 
226  double x = (sin(theta_1) * rho_2 - sin(theta_2) * rho_1) / sin(theta_diff);
227  double y = (cos(theta_2) * rho_1 - cos(theta_1) * rho_2) / sin(theta_diff);
228 
229  s.set_x(x);
230  s.set_y(y);
231  }
232  else if ((vpFeatureVanishingPoint::selectOneOverRho() & select)) {
233  double theta_diff = theta_1 - theta_2;
234  double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
235  double one_over_rho = sin(theta_diff) / denom;
236  // if (one_over_rho < 0) {
237  // one_over_rho = -one_over_rho;
238  // }
239  double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
240 
241  s.setOneOverRho(one_over_rho);
242  s.setAlpha(alpha);
243  }
244  else if ((vpFeatureVanishingPoint::selectAtanOneOverRho() & select)) {
245  double theta_diff = theta_1 - theta_2;
246  double denom = sqrt(rho_1 * rho_1 + rho_2 * rho_2 - 2 * rho_1 * rho_2 * cos(theta_diff));
247  double alpha = atan2(rho_1 * cos(theta_2) - rho_2 * cos(theta_1), rho_2 * sin(theta_1) - rho_1 * sin(theta_2));
248  double one_over_rho = sin(theta_diff) / denom;
249  double atan_one_over_rho = atan2(sin(theta_diff), denom);
250 
251  // if (one_over_rho < 0) {
252  // one_over_rho = -one_over_rho;
253  // }
254 
255  s.setOneOverRho(one_over_rho);
256  s.setAtanOneOverRho(atan_one_over_rho);
257  s.setAlpha(alpha);
258  }
259 }
260 END_VISP_NAMESPACE
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
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 selectAlpha()
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 ...
Definition: vpImagePoint.h:82
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:103
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:79
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:422
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:420