ViSP  2.9.0
vpFeatureBuilderVanishingPoint.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureBuilderVanishingPoint.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Conversion between tracker and visual feature vanishing point.
36  *
37  * Authors:
38  * Odile Bourquardez
39  *
40  *****************************************************************************/
41 
42 
48 #include <visp/vpFeatureBuilder.h>
49 #include <visp/vpFeatureException.h>
50 #include <visp/vpException.h>
51 
52 
62 void
64 {
65  try
66  {
67  s.set_x( t.get_x()) ;
68  s.set_y( t.get_y()) ;
69  }
70  catch(...)
71  {
72  vpERROR_TRACE("Cannot create vanishing point feature") ;
73  throw ;
74  }
75 }
76 
77 
91 void
93 {
94  double rho_l;
95  double rho_r;
96  double theta_l;
97  double theta_r;
98  double c_l;
99  double s_l;
100  double c_r;
101  double s_r;
102 
103  rho_l = L1.getRho();
104  rho_r = L2.getRho();
105  theta_l = L1.getTheta();
106  theta_r = L2.getTheta();
107  c_l = cos(theta_l);
108  c_r = cos(theta_r);
109  s_l = sin(theta_l);
110  s_r = sin(theta_r);
111 
112 
113  double x,y;
114 
115  double min = 0.0001;
116  if(fabs(theta_r-theta_l)<min || fabs(fabs(theta_r-theta_l)-M_PI)<min \
117  || fabs(fabs(theta_r-theta_l)-2*M_PI)<min)
118  {
119  vpCERROR<<"There is no vanishing point : the lines are parallel in the image plane"<<std::endl;
121  "There is no vanishing point : the lines are parallel in the image plane")) ;
122  }
123 
124  y = (rho_r *c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l );
125  x = (rho_r *s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l );
126 
127  s.set_x ( x );
128  s.set_y ( y );
129 }
130 
131 
132 
146 void
148 {
149  vpFeatureLine l1,l2 ;
150  vpFeatureBuilder::create (l1,L1) ;
151  vpFeatureBuilder::create (l2,L2) ;
152 
153  vpFeatureBuilder::create (s, l1, l2) ;
154 
155 }
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpCERROR
Definition: vpDebug.h:369
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
double getRho() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
double getTheta() const
Class that defines what is a point.
Definition: vpPoint.h:65
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:124
Error that can be emited by the vpBasicFeature class and its derivates.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
void set_x(const double _x)
set the point x-coordinates
void set_y(const double _y)
set the point y-coordinates
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)