Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
vpPoseLowe.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 "private/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 /*
49  * MACRO : MIJ
50  *
51  * ENTREE :
52  * m Matrice.
53  * i Indice ligne de l'element.
54  * j Indice colonne de l'element.
55  * s Taille en nombre d'elements d'une ligne de la matrice "m".
56  *
57  * DESCRIPTION :
58  * La macro-instruction calcule l'adresse de l'element de la "i"eme ligne et
59  * de la "j"eme colonne de la matrice "m", soit &m[i][j].
60  *
61  * RETOUR :
62  * L'adresse de m[i][j] est retournee.
63  *
64  * HISTORIQUE :
65  * 1.00 - 11/02/93 - Original.
66  */
67 #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
68 #define NBPTMAX 50
69 #define MINI 0.001
70 #define MINIMUM 0.000001
71 
72 BEGIN_VISP_NAMESPACE
73 
74 // ------------------------------------------------------------------------
75 // FONCTION LOWE :
76 // ------------------------------------------------------------------------
77 // Calcul de la pose pour un objet 3D
78 // ------------------------------------------------------------------------
79 
80 // Je hurle d'horreur devant ces variable globale...
81 static double XI[NBPTMAX], YI[NBPTMAX];
82 static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX];
83 
84 void eval_function(int npt, double *xc, double *f);
85 void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
86 
87 void eval_function(int npt, double *xc, double *f)
88 {
89  int i;
90  const unsigned int sizeU = 3;
91  double u[sizeU];
92  const unsigned int index_0 = 0;
93  const unsigned int index_1 = 1;
94  const unsigned int index_2 = 2;
95  const unsigned int index_3 = 3;
96  const unsigned int index_4 = 4;
97  const unsigned int index_5 = 5;
98 
99  u[index_0] = xc[index_3]; /* Rx */
100  u[index_1] = xc[index_4]; /* Ry */
101  u[index_2] = xc[index_5]; /* Rz */
102 
103  vpRotationMatrix rd(u[index_0], u[index_1], u[index_2]);
104  // --comment: rot_mat(u,rd) matrice de rotation correspondante
105  for (i = 0; i < npt; ++i) {
106  double x = (rd[index_0][index_0] * XO[i]) + (rd[index_0][index_1] * YO[i]) + (rd[index_0][index_2] * ZO[i]) + xc[index_0];
107  double y = (rd[index_1][index_0] * XO[i]) + (rd[index_1][index_1] * YO[i]) + (rd[index_1][index_2] * ZO[i]) + xc[index_1];
108  double z = (rd[index_2][index_0] * XO[i]) + (rd[index_2][index_1] * YO[i]) + (rd[index_2][index_2] * ZO[i]) + xc[index_2];
109  f[i] = (x / z) - XI[i];
110  f[npt + i] = (y / z) - YI[i];
111  // --comment: write fi and fi+1
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  }
150  const int half = 2;
151  npt = m / half;
152 
153  const int flagFunc = 1, flagJacobian = 2;
154  if (iflag == flagFunc) {
155  eval_function(npt, xc, fvecc);
156  }
157  else if (iflag == flagJacobian) {
158  double u1, u2, u3;
159  const unsigned int index_0 = 0;
160  const unsigned int index_1 = 1;
161  const unsigned int index_2 = 2;
162  const unsigned int index_3 = 3;
163  const unsigned int index_4 = 4;
164  const unsigned int index_5 = 5;
165  u[index_0] = xc[index_3];
166  u[index_1] = xc[index_4];
167  u[index_2] = xc[index_5];
168 
169  rd.buildFrom(u[index_0], u[index_1], u[index_2]);
170  /* a partir de l'axe de rotation, calcul de la matrice de rotation. */
171  // --comment: rot_mat of u rd
172 
173  double tt = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2])); /* angle de rot */
174  if (tt >= MINIMUM) {
175  u1 = u[index_0] / tt;
176  u2 = u[index_1] / tt; /* axe de rotation unitaire */
177  u3 = u[index_2] / tt;
178  }
179  else {
180  u1 = 0.0;
181  u2 = 0.0;
182  u3 = 0.0;
183  }
184  double co = cos(tt);
185  double mco = 1.0 - co;
186  double si = sin(tt);
187 
188  for (int i = 0; i < npt; ++i) {
189  double x = XO[i];
190  double y = YO[i]; /* coordonnees du point i */
191  double z = ZO[i];
192 
193  /* coordonnees du point i dans le repere camera */
194  double rx = (rd[index_0][index_0] * x) + (rd[index_0][index_1] * y) + (rd[index_0][index_2] * z) + xc[index_0];
195  double ry = (rd[index_1][index_0] * x) + (rd[index_1][index_1] * y) + (rd[index_1][index_2] * z) + xc[index_1];
196  double rz = (rd[index_2][index_0] * x) + (rd[index_2][index_1] * y) + (rd[index_2][index_2] * z) + xc[index_2];
197 
198  /* derive des fonctions rx, ry et rz par rapport
199  * a tt, u1, u2, u3.
200  */
201  double drxt = (((si * u1 * u3) + (co * u2)) * z) + (((si * u1 * u2) - (co * u3)) * y) + (((si * u1 * u1) - si) * x);
202  double drxu1 = (mco * u3 * z) + (mco * u2 * y) + (2. * mco * u1 * x);
203  double drxu2 = (si * z) + (mco * u1 * y);
204  double drxu3 = (mco * u1 * z) - (si * y);
205 
206  double dryt = (((si * u2 * u3) - (co * u1)) * z) + (((si * u2 * u2) - si) * y) + (((co * u3) + (si * u1 * u2)) * x);
207  double dryu1 = (mco * u2 * x) - (si * z);
208  double dryu2 = (mco * u3 * z) + (2 * mco * u2 * y) + (mco * u1 * x);
209  double dryu3 = (mco * u2 * z) + (si * x);
210 
211  double drzt = (((si * u3 * u3) - si) * z) + (((si * u2 * u3) + (co * u1)) * y) + (((si * u1 * u3) - (co * u2)) * x);
212  double drzu1 = (si * y) + (mco * u3 * x);
213  double drzu2 = (mco * u3 * y) - (si * x);
214  double drzu3 = (2 * mco * u3 * z) + (mco * u2 * y) + (mco * u1 * x);
215 
216  /* derive de la fonction representant le modele de la
217  * camera (sans distortion) par rapport a tt, u1, u2 et u3.
218  */
219  double dxit = (drxt / rz) - ((rx * drzt) / (rz * rz));
220 
221  double dyit = (dryt / rz) - ((ry * drzt) / (rz * rz));
222 
223  double dxiu1 = (drxu1 / rz) - ((drzu1 * rx) / (rz * rz));
224  double dyiu1 = (dryu1 / rz) - ((drzu1 * ry) / (rz * rz));
225 
226  double dxiu2 = (drxu2 / rz) - ((drzu2 * rx) / (rz * rz));
227  double dyiu2 = (dryu2 / rz) - ((drzu2 * ry) / (rz * rz));
228 
229  double dxiu3 = (drxu3 / rz) - ((drzu3 * rx) / (rz * rz));
230  double dyiu3 = (dryu3 / rz) - ((drzu3 * ry) / (rz * rz));
231 
232  /* calcul du jacobien : le jacobien represente la
233  * derivee de la fonction representant le modele de la
234  * camera par rapport aux parametres.
235  */
236  *MIJ(jac, index_0, i, ldfjac) = 1. / rz;
237  *MIJ(jac, index_1, i, ldfjac) = 0.0;
238  *MIJ(jac, index_2, i, ldfjac) = -rx / (rz * rz);
239  if (tt >= MINIMUM) {
240  *MIJ(jac, index_3, i, ldfjac) = (((u1 * dxit) + (((1. - (u1 * u1)) * dxiu1) / tt)) - ((u1 * u2 * dxiu2) / tt)) - ((u1 * u3 * dxiu3) / tt);
241  *MIJ(jac, index_4, i, ldfjac) = (((u2 * dxit) - ((u1 * u2 * dxiu1) / tt)) + (((1. - (u2 * u2)) * dxiu2) / tt)) - ((u2 * u3 * dxiu3) / tt);
242 
243  *MIJ(jac, index_5, i, ldfjac) = (((u3 * dxit) - ((u1 * u3 * dxiu1) / tt)) - ((u2 * u3 * dxiu2) / tt)) + (((1. - (u3 * u3)) * dxiu3) / tt);
244  }
245  else {
246  *MIJ(jac, index_3, i, ldfjac) = 0.0;
247  *MIJ(jac, index_4, i, ldfjac) = 0.0;
248  *MIJ(jac, index_5, i, ldfjac) = 0.0;
249  }
250  *MIJ(jac, index_0, npt + i, ldfjac) = 0.0;
251  *MIJ(jac, index_1, npt + i, ldfjac) = 1 / rz;
252  *MIJ(jac, index_2, npt + i, ldfjac) = -ry / (rz * rz);
253  if (tt >= MINIMUM) {
254  *MIJ(jac, index_3, npt + i, ldfjac) =
255  (((u1 * dyit) + (((1 - (u1 * u1)) * dyiu1) / tt)) - ((u1 * u2 * dyiu2) / tt)) - ((u1 * u3 * dyiu3) / tt);
256  *MIJ(jac, index_4, npt + i, ldfjac) =
257  (((u2 * dyit) - ((u1 * u2 * dyiu1) / tt)) + (((1. - (u2 * u2)) * dyiu2) / tt)) - ((u2 * u3 * dyiu3) / tt);
258  *MIJ(jac, index_5, npt + i, ldfjac) =
259  (((u3 * dyit) - ((u1 * u3 * dyiu1) / tt)) - ((u2 * u3 * dyiu2) / tt)) + (((1. - (u3 * u3)) * dyiu3) / tt);
260  }
261  else {
262  *MIJ(jac, index_3, npt + i, ldfjac) = 0.0;
263  *MIJ(jac, index_4, npt + i, ldfjac) = 0.0;
264  *MIJ(jac, index_5, npt + i, ldfjac) = 0.0;
265  }
266  }
267  } /* fin else if iflag ==2 */
268 }
269 
271 {
272  /* nombre d'elements dans la matrice jac */
273  const int n = NBR_PAR; /* nombres d'inconnues */
274  const int m = static_cast<int>(2 * npt); /* nombres d'equations */
275 
276  const int lwa = (2 * NBPTMAX) + 50; /* taille du vecteur wa */
277  const int ldfjac = 2 * NBPTMAX; /* taille maximum d'une ligne de jac */
278  int info, ipvt[NBR_PAR];
279  int tst_lmder;
280  double f[ldfjac], sol[NBR_PAR];
281  double tol, jac[NBR_PAR][ldfjac], wa[lwa];
282  // --comment: double u of 3 (vecteur de rotation)
283  // --comment: double rd of 3 by 3 (matrice de rotation)
284 
285  tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
286 
287  // --comment: c eq cam
288  // --comment: for i eq 0 to 3
289  // --comment: for j eq 0 to 3 rd[i][j] = cMo[i][j]
290  // --comment: mat_rot of rd and u
291  vpRotationMatrix cRo;
292  cMo.extract(cRo);
293  vpThetaUVector u(cRo);
294  const unsigned int val_3 = 3;
295  for (unsigned int i = 0; i < val_3; ++i) {
296  sol[i] = cMo[i][val_3];
297  sol[i + val_3] = u[i];
298  }
299 
300  vpPoint P;
301  unsigned int i_ = 0;
302  std::list<vpPoint>::const_iterator listp_end = listP.end();
303  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listp_end; ++it) {
304  P = *it;
305  XI[i_] = P.get_x(); // --comment: *cam.px plus cam.xc
306  YI[i_] = P.get_y(); // --comment: *cam.py plus cam.yc
307  XO[i_] = P.get_oX();
308  YO[i_] = P.get_oY();
309  ZO[i_] = P.get_oZ();
310  ++i_;
311  }
312  tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
313  if (tst_lmder == -1) {
314  std::cout << " in CCalculPose::PoseLowe(...) : ";
315  std::cout << "pb de minimization, returns FATAL_ERROR";
316  // --comment: return FATAL ERROR
317  }
318 
319  for (unsigned int i = 0; i < val_3; ++i) {
320  u[i] = sol[i + val_3];
321  }
322 
323  for (unsigned int i = 0; i < val_3; ++i) {
324  cMo[i][val_3] = sol[i];
325  u[i] = sol[i + val_3];
326  }
327 
328  vpRotationMatrix rd(u);
329  cMo.insert(rd);
330 }
331 
332 END_VISP_NAMESPACE
333 
334 #undef MINI
335 #undef MINIMUM
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:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:415
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:426
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:419
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:424
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:417
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:113
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:114
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
Definition: vpPoseLowe.cpp:270
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.