Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPixelMeterConversion.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Pixel to meter conversion.
32  *
33  * Authors:
34  * Eric Marchand
35  * Anthony Saunier
36  *
37  *****************************************************************************/
38 
43 #include<visp3/core/vpPixelMeterConversion.h>
44 #include<visp3/core/vpCameraParameters.h>
45 #include<visp3/core/vpException.h>
46 #include<visp3/core/vpMath.h>
47 #include<visp3/core/vpDebug.h>
48 
49 
51 void
53  const double &rho_p, const double &theta_p,
54  double &rho_m, double &theta_m)
55 {
56  double co = cos(theta_p) ;
57  double si = sin(theta_p) ;
58  double d = vpMath::sqr(cam.px*co)+vpMath::sqr(cam.py*si) ;
59 
60  if (fabs(d)<1e-6)
61  {
62  vpERROR_TRACE("division by zero") ;
64  "division by zero")) ;
65  }
66  theta_m = atan2(si*cam.py, co*cam.px) ;
67  rho_m = (rho_p - cam.u0*co-cam.v0*si)/sqrt(d) ;
68 }
69 
70 
71 void
73  unsigned int order,
74  const vpMatrix &moment_pixel,
75  vpMatrix &moment_meter)
76 {
77 
78  vpMatrix m(order, order);
79  double yc = -cam.v0 ;
80  double xc = -cam.u0 ;
81 
82  for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
83  {
84  for (unsigned int p=0 ; p<order; p++) // iteration en X
85  for (unsigned int q=0; q<order; q++) // iteration en Y
86  if (p+q==k) // on est bien dans la matrice triangulaire superieure
87  {
88  m[p][q] = 0; // initialisation e 0
89  for(unsigned int r=0; r<=p; r++) // somme externe
90  for(unsigned int t=0; t<=q; t++) // somme interne
91  {
92  m[p][q] +=
93  static_cast<double>(vpMath::comb(p, r))
94  * static_cast<double>(vpMath::comb(q, t))
95  * pow(xc, (int)(p-r)) * pow(yc, (int)(q-t))
96  * moment_pixel[r][t];
97 
98  }
99  }
100 
101  }
102 
103  for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
104  for (unsigned int p=0 ; p<order; p++)
105  for (unsigned int q=0; q<order; q++)
106  if (p+q==k)
107  {
108  m[p][q] *= pow(cam.inv_px,(int)(1+p)) * pow(cam.inv_py,(int)(1+q));
109  }
110 
111  for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
112  for (unsigned int p=0 ; p<order; p++)
113  for (unsigned int q=0; q<order; q++)
114  if (p+q==k)
115  {
116  moment_meter[p][q] = m[p][q];
117  }
118 
119 }
120 
121 
122 /*
123  * Local variables:
124  * c-basic-offset: 2
125  * End:
126  */
127 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
static double sqr(double x)
Definition: vpMath.h:110
Generic class defining intrinsic camera parameters.
static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel, vpMatrix &moment_meter)
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:234
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)
line coordinates conversion (rho,theta)