Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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...
79static double XI[NBPTMAX], YI[NBPTMAX];
80static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX];
81
82#define MINI 0.001
83#define MINIMUM 0.000001
84
85void eval_function(int npt, double *xc, double *f);
86void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
87
88void 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 */
135void 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],
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 } else
163 u1 = u2 = u3 = 0.0;
164 double co = cos(tt);
165 double mco = 1.0 - co;
166 double si = sin(tt);
167
168 for (int i = 0; i < npt; i++) {
169 double x = XO[i];
170 double y = YO[i]; /* coordonnees du point i */
171 double z = ZO[i];
172
173 /* coordonnees du point i dans le repere camera */
174 double rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
175 double ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
176 double rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
177
178 /* derive des fonctions rx, ry et rz par rapport
179 * a tt, u1, u2, u3.
180 */
181 double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y + (si * u1 * u1 - si) * x;
182 double drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
183 double drxu2 = si * z + mco * u1 * y;
184 double drxu3 = mco * u1 * z - si * y;
185
186 double dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y + (co * u3 + si * u1 * u2) * x;
187 double dryu1 = mco * u2 * x - si * z;
188 double dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
189 double dryu3 = mco * u2 * z + si * x;
190
191 double drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y + (si * u1 * u3 - co * u2) * x;
192 double drzu1 = si * y + mco * u3 * x;
193 double drzu2 = mco * u3 * y - si * x;
194 double drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
195
196 /* derive de la fonction representant le modele de la
197 * camera (sans distortion) par rapport a tt, u1, u2 et u3.
198 */
199 double dxit = drxt / rz - rx * drzt / (rz * rz);
200
201 double dyit = dryt / rz - ry * drzt / (rz * rz);
202
203 double dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
204 double dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
205
206 double dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
207 double dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
208
209 double dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
210 double dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
211
212 /* calcul du jacobien : le jacobien represente la
213 * derivee de la fonction representant le modele de la
214 * camera par rapport aux parametres.
215 */
216 *MIJ(jac, 0, i, ldfjac) = 1 / rz;
217 *MIJ(jac, 1, i, ldfjac) = 0.0;
218 *MIJ(jac, 2, i, ldfjac) = -rx / (rz * rz);
219 if (tt >= MINIMUM) {
220 *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
221 *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt + (1 - u2 * u2) * dxiu2 / tt - u2 * u3 * dxiu3 / tt;
222
223 *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt + (1 - u3 * u3) * dxiu3 / tt;
224 } else {
225 *MIJ(jac, 3, i, ldfjac) = 0.0;
226 *MIJ(jac, 4, i, ldfjac) = 0.0;
227 *MIJ(jac, 5, i, ldfjac) = 0.0;
228 }
229 *MIJ(jac, 0, npt + i, ldfjac) = 0.0;
230 *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
231 *MIJ(jac, 2, npt + i, ldfjac) = -ry / (rz * rz);
232 if (tt >= MINIMUM) {
233 *MIJ(jac, 3, npt + i, ldfjac) =
234 u1 * dyit + (1 - u1 * u1) * dyiu1 / tt - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
235 *MIJ(jac, 4, npt + i, ldfjac) =
236 u2 * dyit - u1 * u2 * dyiu1 / tt + (1 - u2 * u2) * dyiu2 / tt - u2 * u3 * dyiu3 / tt;
237 *MIJ(jac, 5, npt + i, ldfjac) =
238 u3 * dyit - u1 * u3 * dyiu1 / tt - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
239 } else {
240 *MIJ(jac, 3, npt + i, ldfjac) = 0.0;
241 *MIJ(jac, 4, npt + i, ldfjac) = 0.0;
242 *MIJ(jac, 5, npt + i, ldfjac) = 0.0;
243 }
244 }
245 } /* fin else if iflag ==2 */
246}
247
257{
258#if (DEBUG_LEVEL1)
259 std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
260#endif
261 int n, m; /* nombre d'elements dans la matrice jac */
262 int lwa; /* taille du vecteur wa */
263 int ldfjac; /* taille maximum d'une ligne de jac */
264 int info, ipvt[NBR_PAR];
265 int tst_lmder;
266 double f[2 * NBPTMAX], sol[NBR_PAR];
267 double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
268 // double u[3]; /* vecteur de rotation */
269 // double rd[3][3]; /* matrice de rotation */
270
271 n = NBR_PAR; /* nombres d'inconnues */
272 m = (int)(2 * npt); /* nombres d'equations */
273 lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
274 ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
275 tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
276
277 // c = cam ;
278 // for (i=0;i<3;i++)
279 // for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
280 // mat_rot(rd,u);
282 cMo.extract(cRo);
283 vpThetaUVector u(cRo);
284 for (unsigned int i = 0; i < 3; i++) {
285 sol[i] = cMo[i][3];
286 sol[i + 3] = u[i];
287 }
288
289 vpPoint P;
290 unsigned int i_ = 0;
291 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
292 P = *it;
293 XI[i_] = P.get_x(); //*cam.px + cam.xc ;
294 YI[i_] = P.get_y(); //;*cam.py + cam.yc ;
295 XO[i_] = P.get_oX();
296 YO[i_] = P.get_oY();
297 ZO[i_] = P.get_oZ();
298 ++i_;
299 }
300 tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
301 if (tst_lmder == -1) {
302 std::cout << " in CCalculPose::PoseLowe(...) : ";
303 std::cout << "pb de minimization, returns FATAL_ERROR";
304 // return FATAL_ERROR ;
305 }
306
307 for (unsigned int i = 0; i < 3; i++)
308 u[i] = sol[i + 3];
309
310 for (unsigned int i = 0; i < 3; i++) {
311 cMo[i][3] = sol[i];
312 u[i] = sol[i + 3];
313 }
314
315 vpRotationMatrix rd(u);
316 cMo.insert(rd);
317 // rot_mat(u,rd);
318 // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
319
320#if (DEBUG_LEVEL1)
321 std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
322#endif
323 // return OK ;
324}
325
326#undef MINI
327#undef MINIMUM
328
329#undef DEBUG_LEVEL1
330
331/*
332 * Local variables:
333 * c-basic-offset: 2
334 * End:
335 */
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:458
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
void poseLowe(vpHomogeneousMatrix &cMo)
Compute the pose using the Lowe non linear approach it consider the minimization of a residual using ...
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.