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
eval_function(int npt,double * xc,double * f)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 */
fcn(int m,int n,double * xc,double * fvecc,double * jac,int ldfjac,int iflag)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
254 /*!
255 \brief Compute the pose using the Lowe non linear approach
256 it consider the minimization of a residual using
257 the levenberg marquartd approach.
258
259 The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
260
261 */
poseLowe(vpHomogeneousMatrix & cMo)262 void vpPose::poseLowe(vpHomogeneousMatrix &cMo)
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 */
342