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