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