ViSP  2.10.0
vpPoseLowe.cpp
1 /****************************************************************************
2 *
3 * $Id: vpPoseLowe.cpp 5107 2015-01-05 07:47:13Z 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 * Pose computation.
36 *
37 * Authors:
38 * Eric Marchand
39 * Francois Chaumette
40 *
41 *****************************************************************************/
42 
43 #include <math.h>
44 #include <float.h>
45 #include <string.h>
46 #include <limits> // numeric_limits
47 
48 // besoin de la librairie mathematique, en particulier des
49 // fonctions de minimisation de Levenberg Marquartd
50 #include <visp/vpLevenbergMarquartd.h>
51 #include <visp/vpPose.h>
52 
53 #define NBR_PAR 6
54 #define X3_SIZE 3
55 #define MINIMUM 0.000001
56 
57 #define DEBUG_LEVEL1 0
58 
59 // ------------------------------------------------------------------------
60 // FONCTION LOWE :
61 // ------------------------------------------------------------------------
62 // Calcul de la pose pour un objet 3D
63 // ------------------------------------------------------------------------
64 
65 /*
66 * MACRO : MIJ
67 *
68 * ENTREE :
69 * m Matrice.
70 * i Indice ligne de l'element.
71 * j Indice colonne de l'element.
72 * s Taille en nombre d'elements d'une ligne de la matrice "m".
73 *
74 * DESCRIPTION :
75 * La macro-instruction calcule l'adresse de l'element de la "i"eme ligne et
76 * de la "j"eme colonne de la matrice "m", soit &m[i][j].
77 *
78 * RETOUR :
79 * L'adresse de m[i][j] est retournee.
80 *
81 * HISTORIQUE :
82 * 1.00 - 11/02/93 - Original.
83 */
84 #define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
85 #define NBPTMAX 50
86 
87 // Je hurle d'horreur devant ces variable globale...
88 static double XI[NBPTMAX],YI[NBPTMAX];
89 static double XO[NBPTMAX],YO[NBPTMAX],ZO[NBPTMAX];
90 
91 
92 #define MINI 0.001
93 #define MINIMUM 0.000001
94 
95 void eval_function(int npt,double *xc,double *f);
96 void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
97 
98 void eval_function(int npt,double *xc,double *f)
99 {
100  int i;
101  double x,y,z,u[3];
102 
103  u[0] = xc[3]; /* Rx */
104  u[1] = xc[4]; /* Ry */
105  u[2] = xc[5]; /* Rz */
106 
107  vpRotationMatrix rd(u[0],u[1],u[2]) ;
108  // rot_mat(u,rd); /* matrice de rotation correspondante */
109  for (i=0;i<npt;i++)
110  {
111  x = rd[0][0]*XO[i] + rd[0][1]*YO[i] + rd[0][2]*ZO[i] + xc[0];
112  y = rd[1][0]*XO[i] + rd[1][1]*YO[i] + rd[1][2]*ZO[i] + xc[1];
113  z = rd[2][0]*XO[i] + rd[2][1]*YO[i] + rd[2][2]*ZO[i] + xc[2];
114  f[i] = x/z - XI[i];
115  f[npt+i] = y/z - YI[i];
116  // std::cout << f[i] << " " << f[i+1] << std::endl ;
117  }
118 }
119 
120 
121 /*
122 * PROCEDURE : fcn
123 *
124 * ENTREES :
125 * m Nombre d'equations.
126 * n Nombre de variables.
127 * xc Valeur courante des parametres.
128 * fvecc Resultat de l'evaluation de la fonction.
129 * ldfjac Plus grande dimension de la matrice jac.
130 * iflag Choix du calcul de la fonction ou du jacobien.
131 *
132 * SORTIE :
133 * jac Jacobien de la fonction.
134 *
135 * DESCRIPTION :
136 * La procedure calcule la fonction et le jacobien.
137 * Si iflag == 1, la procedure calcule la fonction en "xc" et le resultat est
138 * stocke dans "fvecc" et "fjac" reste inchange.
139 * Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
140 * stocke dans "fjac" et "fvecc" reste inchange.
141 *
142 * HISTORIQUE :
143 * 1.00 - xx/xx/xx - Original.
144 * 1.01 - 06/07/95 - Modifications.
145 * 2.00 - 24/10/95 - Tableau jac monodimensionnel.
146 */
147 void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
148 {
149  double u[X3_SIZE], rx, ry, rz;// rd[X3_SIZE][X3_SIZE],
150  double tt, mco, co, si, u1, u2, u3, x, y, z;
151  double drxt, drxu1, drxu2, drxu3;
152  double dryt, dryu1, dryu2, dryu3;
153  double drzt, drzu1, drzu2, drzu3;
154  double dxit, dxiu1, dxiu2, dxiu3;
155  double dyit, dyiu1, dyiu2, dyiu3;
156 
157  vpRotationMatrix rd ;
158  int i, npt;
159 
160  if (m < n) printf("pas assez de points\n");
161  npt = m / 2;
162 
163  if (iflag == 1) eval_function (npt, xc, fvecc);
164  else if (iflag == 2)
165  {
166  u[0] =xc[3];
167  u[1]= xc[4];
168  u[2]= xc[5];
169 
170  rd.buildFrom(u[0],u[1],u[2]) ;
171  /* a partir de l'axe de rotation, calcul de la matrice de rotation. */
172  // rot_mat(u, rd);
173 
174  tt = sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */
175  if (tt >= MINIMUM)
176  {
177  u1 = u[0] / tt;
178  u2 = u[1] / tt; /* axe de rotation unitaire */
179  u3 = u[2] / tt;
180  }
181  else u1 = u2 = u3 = 0.0;
182  co = cos(tt);
183  mco = 1.0 - co;
184  si = sin(tt);
185 
186  for (i = 0; i < npt; i++)
187  {
188  x = XO[i];
189  y = YO[i]; /* coordonnees du point i */
190  z = ZO[i];
191 
192  /* coordonnees du point i dans le repere camera */
193  rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
194  ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
195  rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
196 
197  /* derive des fonctions rx, ry et rz par rapport
198  * a tt, u1, u2, u3.
199  */
200  drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y
201  + (si * u1 * u1 - si) * x;
202  drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
203  drxu2 = si * z + mco * u1 * y;
204  drxu3 = mco * u1 * z - si * y;
205 
206  dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y
207  + (co * u3 + si * u1 * u2) * x;
208  dryu1 = mco * u2 * x - si * z;
209  dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
210  dryu3 = mco * u2 * z + si * x;
211 
212  drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y
213  + (si * u1 * u3 - co * u2) * x;
214  drzu1 = si * y + mco * u3 * x;
215  drzu2 = mco * u3 * y - si * x;
216  drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
217 
218  /* derive de la fonction representant le modele de la
219  * camera (sans distortion) par rapport a tt, u1, u2 et u3.
220  */
221  dxit = drxt / rz - rx * drzt / (rz * rz);
222 
223  dyit = dryt / rz - ry * drzt / (rz * rz);
224 
225  dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
226  dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
227 
228  dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
229  dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
230 
231  dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
232  dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
233 
234  /* calcul du jacobien : le jacobien represente la
235  * derivee de la fonction representant le modele de la
236  * camera par rapport aux parametres.
237  */
238  *MIJ(jac, 0, i, ldfjac) = 1 / rz;
239  *MIJ(jac, 1, i, ldfjac) = 0.0;
240  *MIJ(jac, 2, i, ldfjac) = - rx / (rz * rz);
241  if (tt >= MINIMUM)
242  {
243  *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt
244  - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
245  *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt
246  + (1 - u2 * u2) * dxiu2 / tt- u2 * u3 * dxiu3 / tt;
247 
248  *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt
249  + (1 - u3 * u3) * dxiu3 / tt;
250  }
251  else
252  {
253  *MIJ(jac, 3, i, ldfjac) = 0.0;
254  *MIJ(jac, 4, i, ldfjac) = 0.0;
255  *MIJ(jac, 5, i, ldfjac) = 0.0;
256  }
257  *MIJ(jac, 0, npt + i, ldfjac) = 0.0;
258  *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
259  *MIJ(jac, 2, npt + i, ldfjac) = - ry / (rz * rz);
260  if (tt >= MINIMUM)
261  {
262  *MIJ(jac, 3, npt + i, ldfjac) = u1 * dyit + (1 - u1 * u1) * dyiu1 / tt
263  - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
264  *MIJ(jac, 4, npt + i, ldfjac) = u2 * dyit - u1 * u2 * dyiu1 / tt
265  + (1 - u2 * u2) * dyiu2 / tt- u2 * u3 * dyiu3 / tt;
266  *MIJ(jac, 5, npt + i, ldfjac) = u3 * dyit - u1 * u3 * dyiu1 / tt
267  - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
268  }
269  else
270  {
271  *MIJ(jac, 3, npt + i, ldfjac) = 0.0;
272  *MIJ(jac, 4, npt + i, ldfjac) = 0.0;
273  *MIJ(jac, 5, npt + i, ldfjac) = 0.0;
274  }
275  }
276  } /* fin else if iflag ==2 */
277 }
278 
287 void
289 {
290 #if (DEBUG_LEVEL1)
291  std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
292 #endif
293  int n, m; /* nombre d'elements dans la matrice jac */
294  int lwa; /* taille du vecteur wa */
295  int ldfjac; /* taille maximum d'une ligne de jac */
296  int info, ipvt[NBR_PAR];
297  int tst_lmder;
298  double f[2 * NBPTMAX], sol[NBR_PAR];
299  double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
300  // double u[3]; /* vecteur de rotation */
301  // double rd[3][3]; /* matrice de rotation */
302 
303  n = NBR_PAR; /* nombres d'inconnues */
304  m = (int)(2 * npt); /* nombres d'equations */
305  lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
306  ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
307  tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
308 
309  // c = cam ;
310  // for (i=0;i<3;i++)
311  // for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
312  // mat_rot(rd,u);
313  vpRotationMatrix cRo ;
314  cMo.extract(cRo) ;
315  vpThetaUVector u(cRo) ;
316  for (unsigned int i=0;i<3;i++)
317  {
318  sol[i] = cMo[i][3];
319  sol[i+3] = u[i];
320  }
321 
322  vpPoint P ;
323  unsigned int i_=0;
324  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
325  {
326  P = *it;
327  XI[i_] = P.get_x();//*cam.px + cam.xc ;
328  YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
329  XO[i_] = P.get_oX();
330  YO[i_] = P.get_oY();
331  ZO[i_] = P.get_oZ();
332  ++i_;
333  }
334  tst_lmder = lmder1 (&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
335  if (tst_lmder == -1)
336  {
337  std::cout << " in CCalculPose::PoseLowe(...) : " ;
338  std::cout << "pb de minimisation, returns FATAL_ERROR";
339  // return FATAL_ERROR ;
340  }
341 
342  for (unsigned int i = 0; i < 3; i++)
343  u[i] = sol[i + 3];
344 
345  for (unsigned int i=0;i<3;i++)
346  {
347  cMo[i][3] = sol[i];
348  u[i] = sol[i+3];
349  }
350 
351  vpRotationMatrix rd(u) ;
352  cMo.insert(rd) ;
353  // rot_mat(u,rd);
354  // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
355 
356 #if (DEBUG_LEVEL1)
357  std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
358 #endif
359  // return OK ;
360 }
361 
362 
363 #undef MINI
364 #undef MINIMUM
365 
366 
367 #undef DEBUG_LEVEL1
368 
369 
370 /*
371 * Local variables:
372 * c-basic-offset: 2
373 * End:
374 */
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.h:129
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
std::list< vpPoint > listP
array of point (use here class vpPoint)
Definition: vpPose.h:95
Class that defines what is a point.
Definition: vpPoint.h:65
The vpRotationMatrix considers the particular case of a rotation matrix.
void insert(const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Build a rotation matrix from an homogeneous matrix.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.h:131
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:288
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.h:127
Class that consider the case of the parameterization for the rotation.