Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpLevenbergMarquartd.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 * Levenberg Marquartd.
32 */
33
34#include <algorithm> // (std::min)
35#include <cmath> // std::fabs
36#include <iostream>
37#include <limits> // numeric_limits
38
39#include <visp3/core/vpMath.h>
40#include <visp3/vision/vpLevenbergMarquartd.h>
41
42#define SIGN(x) ((x) < 0 ? -1 : 1)
43#define SWAP(a, b, c) \
44 { \
45 (c) = (a); \
46 (a) = (b); \
47 (b) = (c); \
48 }
49#define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
50#define TRUE 1
51#define FALSE 0
52
53/*
54 * PROCEDURE : enorm
55 *
56 * ENTREE :
57 *
58 * x Vecteur de taille "n"
59 * n Taille du vecteur "x"
60 *
61 * DESCRIPTION :
62 * La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n"
63 * La norme euclidienne est calculee par accumulation de la somme des carres
64 * dans les trois directions. Les sommes des carres pour les petits et grands
65 * elements sont mis a echelle afin d'eviter les overflows. Des underflows non
66 * destructifs sont autorisee. Les underflows et overflows sont evites dans le
67 * calcul des sommes des carres non encore mis a echelle par les elements
68 * intermediaires. La definition des elements petit, intermediaire et grand
69 * depend de deux constantes : rdwarf et rdiant. Les restrictions principales
70 * sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est
71 * pas en overflow. Les constantes donnees ici conviennent pour la plupart des
72 * pc connus.
73 *
74 * RETOUR :
75 * En cas de succes, la valeur retournee est la norme euclidienne du vecteur
76 * Sinon, la valeur -1 est retournee et la variable globale "errno" est
77 * initialisee pour indiquee le type de l'erreur.
78 *
79 */
80double enorm(const double *x, int n)
81{
82 const double rdwarf = 3.834e-20;
83 const double rgiant = 1.304e19;
84
85 int i;
86 double agiant, floatn;
87 double norm_eucl = 0.0;
88 double s1 = 0.0, s2 = 0.0, s3 = 0.0;
89 double x1max = 0.0, x3max = 0.0;
90
91 floatn = (double)n;
92 agiant = rgiant / floatn;
93
94 for (i = 0; i < n; i++) {
95 double xabs = std::fabs(x[i]);
96 if ((xabs > rdwarf) && (xabs < agiant)) {
97 /*
98 * somme pour elements intemediaires.
99 */
100 s2 += xabs * xabs;
101 }
102
103 else if (xabs <= rdwarf) {
104 /*
105 * somme pour elements petits.
106 */
107 if (xabs <= x3max) {
108 // if (xabs != 0.0)
109 if (xabs > std::numeric_limits<double>::epsilon())
110 s3 += (xabs / x3max) * (xabs / x3max);
111 } else {
112 s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
113 x3max = xabs;
114 }
115 }
116
117 else {
118 /*
119 * somme pour elements grand.
120 */
121 if (xabs <= x1max) {
122 s1 += (xabs / x1max) * (xabs / x1max);
123 } else {
124 s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
125 x1max = xabs;
126 }
127 }
128 }
129
130 /*
131 * calcul de la norme.
132 */
133 // if (s1 == 0.0)
134 if (std::fabs(s1) <= std::numeric_limits<double>::epsilon()) {
135 // if (s2 == 0.0)
136 if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
137 norm_eucl = x3max * sqrt(s3);
138 else if (s2 >= x3max)
139 norm_eucl = sqrt(s2 * (1.0 + (x3max / s2) * (x3max * s3)));
140 else /*if (s2 < x3max)*/
141 norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
142 } else
143 norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max);
144
145 return (norm_eucl);
146}
147
148/* PROCEDURE : lmpar
149 *
150 * ENTREE :
151 * n Ordre de la matrice "r".
152 * r Matrice de taille "n" x "n". En entree, la toute la partie
153 * triangulaire superieure doit contenir toute la partie
154 *triangulaire superieure de "r".
155 *
156 * ldr Taille maximum de la matrice "r". "ldr" >= "n".
157 *
158 * ipvt Vecteur de taille "n" qui definit la matrice de permutation
159 *"p" tel que : a * p = q * r. La jeme colonne de p la colonne ipvt[j] de la
160 *matrice d'identite.
161 *
162 * diag Vecteur de taille "n" contenant les elements diagonaux de la
163 * matrice "d".
164 *
165 * qtb Vecteur de taille "n" contenant les "n" premiers elements du
166 * vecteur (q transpose)*b.
167 *
168 * delta Limite superieure de la norme euclidienne de d * x.
169 *
170 * par Estimee initiale du parametre de Levenberg-Marquardt.
171 * wa1, wa2 Vecteurs de taille "n" de travail.
172 *
173 * DESCRIPTION :
174 * La procedure determine le parametre de Levenberg-Marquardt. Soit une
175 *matrice "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de
176 *taille "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta,
177 *le probleme est le calcul du parametre "par" de telle facon que si "x"
178 *resoud le systeme
179 *
180 * a * x = b , sqrt(par) * d * x = 0 ,
181 *
182 * au sens des moindre carre, et dxnorm est la norme euclidienne de d * x
183 * alors "par" vaut 0.0 et (dxnorm - delta) <= 0.1 * delta ,
184 * ou "par" est positif et abs(dxnorm-delta) <= 0.1 * delta.
185 * Cette procedure complete la solution du probleme si on lui fourni les infos
186 * nessaires de la factorisation qr, avec pivotage de colonnes de a.
187 * Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
188 * de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
189 * avec les elements diagonaux classes par ordre decroissant de leur valeur,
190 *lmpar attend une matrice triangulaire superieure complete, la matrice de
191 *permutation "p" et les "n" premiers elements de (q transpose) * b. En
192 *sortie, la procedure lmpar fournit aussi une matrice triangulaire
193 *superieure "s" telle que
194 *
195 * t t t
196 * p * (a * a + par * d * d )* p = s * s .
197 *
198 * "s" est utilise a l'interieure de lmpar et peut etre d'un interet separe.
199 *
200 * Seulement quelques iterations sont necessaire pour la convergence de
201 * l'algorithme. Si neanmoins la limite de 10 iterations est atteinte, la
202 * valeur de sortie "par" aura la derniere meilleure valeur.
203 *
204 * SORTIE :
205 * r En sortie, tout le triangle superieur est inchange, et le
206 * le triangle inferieur contient les elements de la partie
207 * triangulaire superieure (transpose) de la matrice triangulaire
208 * superieure de "s".
209 * par Estimee finale du parametre de Levenberg-Marquardt.
210 * x Vecteur de taille "n" contenant la solution au sens des
211 *moindres carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le
212 * parametre en sortie "par"
213 * sdiag Vecteur de taille "n" contenant les elements diagonaux de la
214 * matrice triangulaire "s".
215 *
216 * RETOUR :
217 * En cas de succes, la valeur 0.0 est retournee.
218 *
219 */
220int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *delta, double *par, double *x,
221 double *sdiag, double *wa1, double *wa2)
222{
223 const double tol1 = 0.1; /* tolerance a 0.1 */
224
225 int l;
226 unsigned int iter; /* compteur d'iteration */
227 int nsing; /* nombre de singularite de la matrice */
228 double dxnorm, fp;
229 double temp;
230 double dwarf = DBL_MIN; /* plus petite amplitude positive */
231
232 /*
233 * calcul et stockage dans "x" de la direction de Gauss-Newton. Si
234 * le jacobien a une rangee de moins, on a une solution au moindre
235 * carres.
236 */
237 nsing = n;
238
239 for (int i = 0; i < n; i++) {
240 wa1[i] = qtb[i];
241 double *pt = MIJ(r, i, i, ldr);
242 // if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
243 if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
244 nsing = i - 1;
245
246 if (nsing < n)
247 wa1[i] = 0.0;
248 }
249
250 if (nsing >= 0) {
251 for (int k = 0; k < nsing; k++) {
252 int i = nsing - 1 - k;
253 wa1[i] /= *MIJ(r, i, i, ldr);
254 temp = wa1[i];
255 int jm1 = i - 1;
256
257 if (jm1 >= 0) {
258 for (unsigned int j = 0; j <= (unsigned int)jm1; j++)
259 wa1[j] -= *MIJ(r, i, j, ldr) * temp;
260 }
261 }
262 }
263
264 for (int j = 0; j < n; j++) {
265 l = ipvt[j];
266 x[l] = wa1[j];
267 }
268
269 /*
270 * initialisation du compteur d'iteration.
271 * evaluation de la fonction a l'origine, et test
272 * d'acceptation de la direction de Gauss-Newton.
273 */
274 iter = 0;
275
276 for (int i = 0; i < n; i++)
277 wa2[i] = diag[i] * x[i];
278
279 dxnorm = enorm(wa2, n);
280
281 fp = dxnorm - *delta;
282
283 if (fp > tol1 * (*delta)) {
284 /*
285 * Si le jacobien n'a pas de rangee deficiente,l'etape de
286 * Newton fournit une limite inferieure, parl pour le
287 * zero de la fonction. Sinon cette limite vaut 0.0.
288 */
289 double parl = 0.0;
290
291 if (nsing >= n) {
292 for (int i = 0; i < n; i++) {
293 l = ipvt[i];
294 wa1[i] = diag[l] * (wa2[l] / dxnorm);
295 }
296
297 for (int i = 0; i < n; i++) {
298 long im1;
299 double sum = 0.0;
300 im1 = (i - 1L);
301
302 if (im1 >= 0) {
303 for (unsigned int j = 0; j <= (unsigned int)im1; j++)
304 sum += (*MIJ(r, i, j, ldr) * wa1[j]);
305 }
306 wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
307 }
308
309 temp = enorm(wa1, n);
310 parl = ((fp / *delta) / temp) / temp;
311 }
312
313 /*
314 * calcul d'une limite superieure, paru, pour le zero de la
315 * fonction.
316 */
317 for (int i = 0; i < n; i++) {
318 double sum = 0.0;
319
320 for (int j = 0; j <= i; j++)
321 sum += *MIJ(r, i, j, ldr) * qtb[j];
322
323 l = ipvt[i];
324 wa1[i] = sum / diag[l];
325 }
326
327 double gnorm = enorm(wa1, n);
328 double paru = gnorm / *delta;
329
330 // if (paru == 0.0)
331 if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
332 paru = dwarf / vpMath::minimum(*delta, tol1);
333
334 /*
335 * Si "par" en entree tombe hors de l'intervalle (parl,paru),
336 * on le prend proche du point final.
337 */
338
339 *par = vpMath::maximum(*par, parl);
340 *par = vpMath::maximum(*par, paru);
341
342 // if (*par == 0.0)
343 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon())
344 *par = gnorm / dxnorm;
345
346 /*
347 * debut d'une iteration.
348 */
349 for (;;) // iter >= 0)
350 {
351 iter++;
352
353 /*
354 * evaluation de la fonction a la valeur courant
355 * de "par".
356 */
357 // if (*par == 0.0)
358 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon()) {
359 const double tol001 = 0.001; /* tolerance a 0.001 */
360 *par = vpMath::maximum(dwarf, (tol001 * paru));
361 }
362
363 temp = sqrt(*par);
364
365 for (int i = 0; i < n; i++)
366 wa1[i] = temp * diag[i];
367
368 qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
369
370 for (int i = 0; i < n; i++)
371 wa2[i] = diag[i] * x[i];
372
373 dxnorm = enorm(wa2, n);
374 temp = fp;
375 fp = dxnorm - *delta;
376
377 /*
378 * si la fonction est assez petite, acceptation de
379 * la valeur courant de "par". de plus, test des cas
380 * ou parl est nul et ou le nombre d'iteration a
381 * atteint 10.
382 */
383 // if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <=
384 // temp)
385 // && (temp < 0.0)) || (iter == 10))
386 if ((std::fabs(fp) <= tol1 * (*delta)) ||
387 ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp) && (temp < 0.0)) ||
388 (iter == 10)) {
389 // terminaison.
390
391 // Remove the two next lines since this is a dead code
392 /* if (iter == 0)
393 *par = 0.0; */
394
395 return (0);
396 }
397
398 /*
399 * calcul de la correction de Newton.
400 */
401
402 for (int i = 0; i < n; i++) {
403 l = ipvt[i];
404 wa1[i] = diag[l] * (wa2[l] / dxnorm);
405 }
406
407 for (unsigned int i = 0; i < (unsigned int)n; i++) {
408 wa1[i] = wa1[i] / sdiag[i];
409 temp = wa1[i];
410 unsigned int jp1 = i + 1;
411 if ((unsigned int)n >= jp1) {
412 for (unsigned int j = jp1; j < (unsigned int)n; j++)
413 wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
414 }
415 }
416
417 temp = enorm(wa1, n);
418 double parc = ((fp / *delta) / temp) / temp;
419
420 /*
421 * selon le signe de la fonction, mise a jour
422 * de parl ou paru.
423 */
424 if (fp > 0.0)
425 parl = vpMath::maximum(parl, *par);
426
427 if (fp < 0.0)
428 paru = vpMath::minimum(paru, *par);
429
430 /*
431 * calcul d'une estimee ameliree de "par".
432 */
433 *par = vpMath::maximum(parl, (*par + parc));
434 } /* fin boucle sur iter */
435 } /* fin fp > tol1 * delta */
436
437 /*
438 * terminaison.
439 */
440 if (iter == 0)
441 *par = 0.0;
442
443 return (0);
444}
445
446/*
447 * PROCEDURE : pythag
448 *
449 * ENTREES :
450 * a, b Variables dont on veut la racine carre de leur somme de carre
451 *
452 * DESCRIPTION :
453 * La procedure calcule la racine carre de la somme des carres de deux nombres
454 * en evitant l'overflow ou l'underflow destructif.
455 *
456 * RETOUR :
457 * La procedure retourne la racine carre de a^2 + b^2.
458 *
459 */
460double pythag(double a, double b)
461{
462 double pyth, p, r, t;
463
464 p = vpMath::maximum(std::fabs(a), std::fabs(b));
465
466 // if (p == 0.0)
467 if (std::fabs(p) <= std::numeric_limits<double>::epsilon()) {
468 pyth = p;
469 return (pyth);
470 }
471
472 r = ((std::min)(std::fabs(a), std::fabs(b)) / p) * ((std::min)(std::fabs(a), std::fabs(b)) / p);
473 t = 4.0 + r;
474
475 // while (t != 4.0)
476 while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits<double>::epsilon()) {
477 double s = r / t;
478 double u = 1.0 + 2.0 * s;
479 p *= u;
480 r *= (s / u) * (s / u);
481 t = 4.0 + r;
482 }
483
484 pyth = p;
485 return (pyth);
486}
487
488/*
489 * PROCEDURE : qrfac
490 *
491 * ENTREE :
492 * m Nombre de lignes de la matrice "a".
493 * n Nombre de colonne de la matrice "a".
494 * a Matrice de taille "m" x "n". elle contient, en entree la
495 *matrice dont on veut sa factorisation qr.
496 * lda Taille maximale de "a". lda >= m.
497 * pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est
498 *realise Si pivot = FALSE, pas de pivotage.
499 * lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de
500 * l'ordre de 1. Sinon lipvt est de l'ordre de "n".
501 * wa Vecteur de travail de taille "n". Si pivot = FALSE "wa"
502 * coincide avec rdiag.
503 *
504 * DESCRIPTION :
505 * La procedure effectue une decomposition de la matrice "a"par la methode qr.
506 * Elle utilise les transformations de householders avec pivotage sur les
507 *colonnes (option) pour calculer la factorisation qr de la matrice "a" de
508 *taille "m" x "n". La procedure determine une matrice orthogonale "q", une
509 *matrice de permutation "p" et une matrice trapesoidale superieure "r" dont
510 *les elements diagonaux sont ordonnes dans l'ordre decroissant de leurs
511 *valeurs,tel que a * p = q * r. La transformation de householder pour la
512 *colonne k, k = 1,2,...,min(m,n), est de la forme
513 * t
514 * i - (1 / u(k)) * u * u
515 *
516 * Ou u a des zeros dans les k-1 premieres positions.
517 *
518 * SORTIE :
519 * a Matrice de taille "m" x "n" dont le trapeze superieur de "a"
520 * contient la partie trapezoidale superieure de "r" et la partie
521 * trapezoidale inferieure de "a" contient une forme factorisee
522 * de "q" (les elements non triviaux du vecteurs "u" sont decrits
523 * ci-dessus).
524 * ipvt Vecteur de taille "n". Il definit la matrice de permutation
525 *"p" tel que a * p = q * r. La jeme colonne de p est la colonne ipvt[j] de la
526 *matrice d'identite. Si pivot = FALSE, ipvt n'est pas referencee. rdiag
527 *Vecteur de taille "n" contenant les elements diagonaux de la matrice
528 *"r". acnorm Vecteur de taille "n" contenant les normes des lignes
529 * correspondantes de la matrice "a". Si cette information n'est
530 * pas requise, acnorm coincide avec rdiag.
531 *
532 */
533int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt */, double *rdiag, double *acnorm,
534 double *wa)
535{
536 const double tolerance = 0.05;
537
538 int i, j, ip1, k, kmax, minmn;
539 double epsmch;
540 double sum, temp, tmp;
541
542 /*
543 * epsmch est la precision machine.
544 */
545 epsmch = std::numeric_limits<double>::epsilon();
546
547 /*
548 * calcul des normes initiales des lignes et initialisation
549 * de plusieurs tableaux.
550 */
551 for (i = 0; i < m; i++) {
552 acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
553 rdiag[i] = acnorm[i];
554 wa[i] = rdiag[i];
555
556 if (pivot)
557 ipvt[i] = i;
558 }
559 /*
560 * reduction de "a" en "r" avec les tranformations de Householder.
561 */
562 minmn = vpMath::minimum(m, n);
563 for (i = 0; i < minmn; i++) {
564 if (pivot) {
565 /*
566 * met la ligne de plus grande norme en position
567 * de pivot.
568 */
569 kmax = i;
570 for (k = i; k < m; k++) {
571 if (rdiag[k] > rdiag[kmax])
572 kmax = k;
573 }
574
575 if (kmax != i) {
576 for (j = 0; j < n; j++)
577 SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp);
578
579 rdiag[kmax] = rdiag[i];
580 wa[kmax] = wa[i];
581
582 SWAP(ipvt[i], ipvt[kmax], k);
583 }
584 }
585
586 /*
587 * calcul de al transformationde Householder afin de reduire
588 * la jeme ligne de "a" a un multiple du jeme vecteur unite.
589 */
590 double ajnorm = enorm(MIJ(a, i, i, lda), n - i);
591
592 // if (ajnorm != 0.0)
593 if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon()) {
594 if (*MIJ(a, i, i, lda) < 0.0)
595 ajnorm = -ajnorm;
596
597 for (j = i; j < n; j++)
598 *MIJ(a, i, j, lda) /= ajnorm;
599 *MIJ(a, i, i, lda) += 1.0;
600
601 /*
602 * application de la tranformation aux lignes
603 * restantes et mise a jour des normes.
604 */
605 ip1 = i + 1;
606
607 if (m >= ip1) {
608 for (k = ip1; k < m; k++) {
609 sum = 0.0;
610 for (j = i; j < n; j++)
611 sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
612
613 temp = sum / *MIJ(a, i, i, lda);
614
615 for (j = i; j < n; j++)
616 *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
617
618 // if (pivot && rdiag[k] != 0.0)
619 if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon())) {
620 temp = *MIJ(a, k, i, lda) / rdiag[k];
621 rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
622
623 if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch) {
624 rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - (int)i));
625 wa[k] = rdiag[k];
626 }
627 }
628 } /* fin boucle for k */
629 }
630
631 } /* fin if (ajnorm) */
632
633 rdiag[i] = -ajnorm;
634 } /* fin for (i = 0; i < minmn; i++) */
635 return (0);
636}
637
638/* PROCEDURE : qrsolv
639 *
640 * ENTREE :
641 * n Ordre de la matrice "r".
642 * r Matrice de taille "n" x "n". En entree, la partie triangulaire
643 * complete de "r" doit contenir la partie triangulaire
644 *superieure complete de "r".
645 * ldr Taille maximale de la matrice "r". "ldr" >= n.
646 * ipvt Vecteur de taille "n" definissant la matrice de permutation
647 *"p" La jeme colonne de de "p" est la colonne ipvt[j] de la matrice identite.
648 * diag Vecteur de taille "n" contenant les elements diagonaux de la
649 * matrice "d".
650 * qtb Vecteur de taille "n" contenant les "n" premiers elements du
651 * vecteur (q transpose) * b.
652 * wa Vecteur de travail de taille "n".
653 *
654 * DESCRIPTION :
655 * La procedure complete la solution du probleme, si on fournit les
656 *information necessaires de la factorisation qr, avec pivotage des colonnes.
657 * Soit une matrice "a" de taille "m" x "n" donnee, une matrice diagonale "d"
658 *de taille "n" x "n" et un vecteur "b" de taille "m", le probleme est la
659 *determination un vecteur "x" qui est solution du systeme
660 *
661 * a*x = b , d*x = 0 ,
662 *
663 * Au sens des moindres carres.
664 *
665 * Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de
666 *"q" sont orthogonales et "r" est une matrice traingulaire superieure dont
667 *les elements diagonaux sont classes de l'ordre decroissant de leur valeur.
668 *Cette procedure attend donc la matrice triangulaire superieure remplie "r",
669 *la matrice de permutaion "p" et les "n" premiers elements de (q transpose)
670 ** b. Le systeme
671 *
672 * a * x = b, d * x = 0, est alors equivalent a
673 *
674 * t t
675 * r * z = q * b , p * d * p * z = 0 ,
676 *
677 * Ou x = p * z. Si ce systeme ne possede pas de rangee pleine, alors une
678 * solution au moindre carre est obtenue. En sortie, la procedure fournit
679 *aussi une matrice triangulaire superieure "s" tel que
680 *
681 * t t t
682 * p * (a * a + d * d) * p = s * s .
683 *
684 * "s" est calculee a l'interieure de qrsolv et peut etre hors interet.
685 *
686 * SORTIE :
687 * r En sortie, le triangle superieur n'est pas altere, et la
688 *partie triangulaire inferieure contient la partie triangulaire superieure
689 * (transpose) de la matrice triangulaire "s".
690 * x Vecteur de taille "n" contenant les solutions au moindres
691 *carres du systeme a * x = b, d * x = 0. sdiag Vecteur de taille "n"
692 *contenant les elements diagonaux de la matrice triangulaire superieure "s".
693 *
694 */
695int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa)
696{
697 int i, j, k, kp1, l; /* compteur de boucle */
698 int nsing;
699 double cosi, cotg, qtbpj, sinu, tg, temp;
700
701 /*
702 * copie de r et (q transpose) * b afin de preserver l'entree
703 * et initialisation de "s". En particulier, sauvegarde des elements
704 * diagonaux de "r" dans "x".
705 */
706 for (i = 0; i < n; i++) {
707 for (j = i; j < n; j++)
708 *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
709
710 x[i] = *MIJ(r, i, i, ldr);
711 wa[i] = qtb[i];
712 }
713
714 /*
715 * Elimination de la matrice diagonale "d" en utlisant une rotation
716 * connue.
717 */
718
719 for (i = 0; i < n; i++) {
720 /*
721 * preparation de la colonne de d a eliminer, reperage de
722 * l'element diagonal par utilisation de p de la
723 * factorisation qr.
724 */
725 l = ipvt[i];
726
727 // if (diag[l] != 0.0)
728 if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon()) {
729 for (k = i; k < n; k++)
730 sdiag[k] = 0.0;
731
732 sdiag[i] = diag[l];
733
734 /*
735 * Les transformations qui eliminent la colonne de d
736 * modifient seulement qu'un seul element de
737 * (q transpose)*b avant les n premiers elements
738 * lesquels sont inialement nuls.
739 */
740
741 qtbpj = 0.0;
742
743 for (k = i; k < n; k++) {
744 /*
745 * determination d'une rotation qui elimine
746 * les elements appropriees dans la colonne
747 * courante de d.
748 */
749
750 // if (sdiag[k] != 0.0)
751 if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon()) {
752 if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k])) {
753 tg = sdiag[k] / *MIJ(r, k, k, ldr);
754 cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
755 sinu = cosi * tg;
756 } else {
757 cotg = *MIJ(r, k, k, ldr) / sdiag[k];
758 sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
759 cosi = sinu * cotg;
760 }
761
762 /*
763 * calcul des elements de la diagonale modifiee
764 * de r et des elements modifies de
765 * ((q transpose)*b,0).
766 */
767 *MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k];
768 temp = cosi * wa[k] + sinu * qtbpj;
769 qtbpj = -sinu * wa[k] + cosi * qtbpj;
770 wa[k] = temp;
771
772 /*
773 * accumulation des tranformations dans
774 * les lignes de s.
775 */
776
777 kp1 = k + 1;
778
779 if (n >= kp1) {
780 for (j = kp1; j < n; j++) {
781 temp = cosi * *MIJ(r, k, j, ldr) + sinu * sdiag[j];
782 sdiag[j] = -sinu * *MIJ(r, k, j, ldr) + cosi * sdiag[j];
783 *MIJ(r, k, j, ldr) = temp;
784 }
785 }
786 } /* fin if diag[] !=0 */
787 } /* fin boucle for k -> n */
788 } /* fin if diag =0 */
789
790 /*
791 * stokage de l'element diagonal de s et restauration de
792 * l'element diagonal correspondant de r.
793 */
794 sdiag[i] = *MIJ(r, i, i, ldr);
795 *MIJ(r, i, i, ldr) = x[i];
796 } /* fin boucle for j -> n */
797
798 /*
799 * resolution du systeme triangulaire pour z. Si le systeme est
800 * singulier, on obtient une solution au moindres carres.
801 */
802 nsing = n;
803
804 for (i = 0; i < n; i++) {
805 // if (sdiag[i] == 0.0 && nsing == n)
806 if ((std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
807 nsing = i - 1;
808
809 if (nsing < n)
810 wa[i] = 0.0;
811 }
812
813 if (nsing >= 0) {
814 for (k = 0; k < nsing; k++) {
815 i = nsing - 1 - k;
816 double sum = 0.0;
817 int jp1 = i + 1;
818
819 if (nsing >= jp1) {
820 for (j = jp1; j < nsing; j++)
821 sum += *MIJ(r, i, j, ldr) * wa[j];
822 }
823 wa[i] = (wa[i] - sum) / sdiag[i];
824 }
825 }
826 /*
827 * permutation arriere des composants de z et des componants de x.
828 */
829
830 for (j = 0; j < n; j++) {
831 l = ipvt[j];
832 x[l] = wa[j];
833 }
834 return (0);
835}
836
837/*
838 * PROCEDURE : lmder
839 *
840 *
841 * ENTREE :
842 * fcn Fonction qui calcule la fonction et le jacobien de la
843 *fonction. m Nombre de fonctions.
844 * n Nombre de variables. n <= m
845 * x Vecteur de taille "n" contenant en entree une estimation
846 * initiale de la solution.
847 * ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m".
848 * ftol Erreur relative desiree dans la somme des carre. La
849 *terminaison survient quand les preductions estimee et vraie de la somme des
850 * carres sont toutes deux au moins egal a ftol.
851 * xtol Erreur relative desiree dans la solution approximee. La
852 * terminaison survient quand l'erreur relative entre deux
853 * iterations consecutives est au moins egal a xtol.
854 * gtol Mesure de l'orthogonalite entre le vecteur des fonctions et
855 *les colonnes du jacobien. La terminaison survient quand le cosinus de
856 *l'angle entre fvec et n'importe quelle colonne du jacobien est au moins
857 *egal a gtol, en valeur absolue. maxfev Nombre d'appel maximum. La
858 *terminaison se produit lorsque le nombre d'appel a fcn avec iflag = 1 a
859 *atteint "maxfev".
860 * diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est
861 * initialisee en interne. Si mode = 2, diag doit contenir les
862 * entree positives qui servent de facteurs d'echelle aux
863 *variables.
864 * mode Si mode = 1, les variables seront mis a l'echelle en interne.
865 * Si mode = 2, la mise a l'echelle est specifie par l'entree
866 *diag. Les autres valeurs de mode sont equivalents a mode = 1. factor
867 *Definit la limite de l'etape initial. Cette limite est initialise au
868 *produit de "factor" et de la norme euclidienne de "diag" * "x" sinon nul.
869 *ou a "factor" lui meme. Dans la plupart des cas, "factor" doit se trouve
870 *dans l'intervalle (1, 100); ou 100 est la valeur recommandee. nprint
871 *Controle de l'impression des iterees (si valeur positive). Dans ce
872 *cas, fcn est appelle avec iflag = 0 au debut de la premiere iteration et
873 *apres chaque nprint iteration, x, fvec, et fjac sont disponible pour
874 *impression, cela avant de quitter la procedure. Si "nprint" est negatif,
875 *aucun appel special de fcn est faite. wa1, wa2, wa3 Vecteur de travail de
876 *taille "n". wa4 Vecteur de travail de taille "m".
877 *
878 *
879 * SORTIE :
880 * x Vecteur de taille "n" contenant en sortie l'estimee finale
881 * de la solution.
882 * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x".
883 * fjac Matrice de taille "m" x "n". La sous matrice superieure de
884 * taille "n" x "n" de fjac contient une matrice triangulaire
885 * superieure r dont les elements diagonaux, classe dans le sens
886 * decroissant de leur valeur, sont de la forme :
887 *
888 * T T T
889 * p * (jac * jac) * p = r * r
890 *
891 * Ou p est une matrice de permutation et jac est le jacobien
892 * final calcule.
893 * La colonne j de p est la colonne ipvt (j) (voir ci apres) de
894 * la matrice identite. La partie trapesoidale inferieure de fjac
895 * contient les information genere durant le calcul de r.
896 * info Information de l'execution de la procedure. Lorsque la
897 *procedure a termine son execution, "info" est inialisee a la valeur
898 * (negative) de iflag. sinon elle prend les valeurs suivantes :
899 * info = 0 : parametres en entree non valides.
900 * info = 1 : les reductions relatives reelle et estimee de la
901 * somme des carres sont au moins egales a ftol.
902 * info = 2 : erreur relative entre deux iteres consecutives sont
903 * egaux a xtol.
904 * info = 3 : conditions info = 1 et info = 2 tous deux requis.
905 * info = 4 : le cosinus de l'angle entre fvec et n'importe
906 *quelle colonne du jacobien est au moins egal a gtol, en valeur absolue. info
907 *= 5 : nombre d'appels a fcn avec iflag = 1 a atteint maxfev. info = 6 :
908 *ftol est trop petit. Plus moyen de reduire de la somme des carres. info =
909 *7 : xtol est trop petit. Plus d'amelioration possible pour approximer la
910 *solution x. info = 8 : gtol est trop petit. "fvec" est orthogonal aux
911 * colonnes du jacobien a la precision machine pres.
912 * nfev Nombre d'appel a "fcn" avec iflag = 1.
913 * njev Nombre d'appel a "fcn" avec iflag = 2.
914 * ipvt Vecteur de taille "n". Il definit une matrice de permutation p
915 * tel que jac * p = q * p, ou jac est le jacbien final calcule,
916 * q est orthogonal (non socke) et r est triangulaire superieur,
917 * avec les elements diagonaux classes en ordre decroissant de
918 * leur valeur. La colonne j de p est ipvt[j] de la matrice
919 *identite. qtf Vecteur de taille n contenant les n premiers elements
920 *du vecteur qT * fvec.
921 *
922 * DESCRIPTION :
923 * La procedure minimize la somme de carre de m equation non lineaire a n
924 * variables par une modification de l'algorithme de Levenberg - Marquardt.
925 *
926 * REMARQUE :
927 * L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et
928 * le jacobien.
929 * "fcn" doit etre declare dans une instruction externe a la procedure et doit
930 * etre appele comme suit :
931 * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
932 **iflag)
933 *
934 * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
935 * fjac n'est pas modifie.
936 * si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac.
937 * fvec n'est pas modifie.
938 *
939 * RETOUR :
940 * En cas de succes, la valeur zero est retournee.
941 * Sinon la valeur -1 est retournee.
942 */
943int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
944 double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev,
945 double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt,
946 double *qtf, double *wa1, double *wa2, double *wa3, double *wa4)
947{
948 const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
949 int oncol = TRUE;
950 int iflag, iter;
951 int count = 0;
952 int i, j, l;
953 double actred, delta, dirder, epsmch, fnorm, fnorm1;
954 double ratio = std::numeric_limits<double>::epsilon();
955 double par, pnorm, prered;
956 double sum, temp, temp1, temp2, xnorm = 0.0;
957
958 /* epsmch est la precision machine. */
959 epsmch = std::numeric_limits<double>::epsilon();
960
961 *info = 0;
962 iflag = 0;
963 *nfev = 0;
964 *njev = 0;
965
966 /* verification des parametres d'entree. */
967
968 /*if (n <= 0)
969 return 0;*/
970 if (m < n)
971 return 0;
972 if (ldfjac < m)
973 return 0;
974 if (ftol < 0.0)
975 return 0;
976 if (xtol < 0.0)
977 return 0;
978 if (gtol < 0.0)
979 return 0;
980 if (maxfev == 0)
981 return 0;
982 if (factor <= 0.0)
983 return 0;
984 if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0) || (maxfev == 0) ||
985 (factor <= 0.0)) {
986 /*
987 * termination, normal ou imposee par l'utilisateur.
988 */
989 if (iflag < 0)
990 *info = iflag;
991
992 iflag = 0;
993
994 if (nprint > 0)
995 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
996
997 return (count);
998 }
999
1000 if (mode == 2) {
1001 for (j = 0; j < n; j++) {
1002 if (diag[j] <= 0.0) {
1003 /*
1004 * termination, normal ou imposee par l'utilisateur.
1005 */
1006 if (iflag < 0)
1007 *info = iflag;
1008
1009 iflag = 0;
1010
1011 if (nprint > 0)
1012 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1013
1014 return (count);
1015 }
1016 }
1017 }
1018
1019 /*
1020 * evaluation de la fonction au point de depart
1021 * et calcul de sa norme.
1022 */
1023 iflag = 1;
1024
1025 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1026
1027 *nfev = 1;
1028
1029 if (iflag < 0) {
1030 /*
1031 * termination, normal ou imposee par l'utilisateur.
1032 */
1033 if (iflag < 0)
1034 *info = iflag;
1035
1036 iflag = 0;
1037
1038 if (nprint > 0)
1039 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1040
1041 return (count);
1042 }
1043
1044 fnorm = enorm(fvec, m);
1045
1046 /*
1047 * initialisation du parametre de Levenberg-Marquardt
1048 * et du conteur d'iteration.
1049 */
1050
1051 par = 0.0;
1052 iter = 1;
1053
1054 /*
1055 * debut de la boucle la plus externe.
1056 */
1057 while (count < (int)maxfev) {
1058 count++;
1059 /*
1060 * calcul de la matrice jacobienne.
1061 */
1062
1063 iflag = 2;
1064
1065 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1066
1067 (*njev)++;
1068
1069 if (iflag < 0) {
1070 /*
1071 * termination, normal ou imposee par l'utilisateur.
1072 */
1073 if (iflag < 0)
1074 *info = iflag;
1075
1076 iflag = 0;
1077
1078 if (nprint > 0)
1079 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1080
1081 return (count);
1082 }
1083
1084 /*
1085 * si demandee, appel de fcn pour impression des iterees.
1086 */
1087 if (nprint > 0) {
1088 iflag = 0;
1089 if ((iter - 1) % nprint == 0)
1090 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1091
1092 if (iflag < 0) {
1093 /*
1094 * termination, normal ou imposee par l'utilisateur.
1095 */
1096 if (iflag < 0)
1097 *info = iflag;
1098
1099 iflag = 0;
1100
1101 if (nprint > 0)
1102 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1103
1104 return (count);
1105 }
1106 }
1107
1108 /*
1109 * calcul de la factorisation qr du jacobien.
1110 */
1111 qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3);
1112
1113 /*
1114 * a la premiere iteration et si mode est 1, mise a l'echelle
1115 * en accord avec les normes des colonnes du jacobien initial.
1116 */
1117
1118 if (iter == 1) {
1119 if (mode != 2) {
1120 for (j = 0; j < n; j++) {
1121 diag[j] = wa2[j];
1122 // if (wa2[j] == 0.0)
1123 if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
1124 diag[j] = 1.0;
1125 }
1126 }
1127
1128 /*
1129 * a la premiere iteration, calcul de la norme de x mis
1130 * a l'echelle et initialisation de la limite delta de
1131 * l'etape.
1132 */
1133
1134 for (j = 0; j < n; j++)
1135 wa3[j] = diag[j] * x[j];
1136
1137 xnorm = enorm(wa3, n);
1138 delta = factor * xnorm;
1139
1140 // if (delta == 0.0)
1141 if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
1142 delta = factor;
1143 }
1144
1145 /*
1146 * formation de (q transpose) * fvec et stockage des n premiers
1147 * composants dans qtf.
1148 */
1149 for (i = 0; i < m; i++)
1150 wa4[i] = fvec[i];
1151
1152 for (i = 0; i < n; i++) {
1153 double *pt = MIJ(fjac, i, i, ldfjac);
1154 // if (*MIJ(fjac, i, i, ldfjac) != 0.0)
1155 if (std::fabs(*pt) > std::numeric_limits<double>::epsilon()) {
1156 sum = 0.0;
1157
1158 for (j = i; j < m; j++)
1159 sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
1160
1161 temp = -sum / *MIJ(fjac, i, i, ldfjac);
1162
1163 for (j = i; j < m; j++)
1164 wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
1165 }
1166
1167 *MIJ(fjac, i, i, ldfjac) = wa1[i];
1168 qtf[i] = wa4[i];
1169 }
1170
1171 /*
1172 * calcul de la norme du gradient mis a l'echelle.
1173 */
1174
1175 double gnorm = 0.0;
1176
1177 // if (fnorm != 0.0)
1178 if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon()) {
1179 for (i = 0; i < n; i++) {
1180 l = ipvt[i];
1181 // if (wa2[l] != 0.0)
1182 if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon()) {
1183 sum = 0.0;
1184 for (j = 0; j <= i; j++)
1185 sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
1186
1187 gnorm = vpMath::maximum(gnorm, std::fabs(sum / wa2[l]));
1188 }
1189 }
1190 }
1191
1192 /*
1193 * test pour la convergence de la norme du gradient .
1194 */
1195
1196 if (gnorm <= gtol)
1197 *info = 4;
1198
1199 if (*info != 0) {
1200 /*
1201 * termination, normal ou imposee par l'utilisateur.
1202 */
1203 if (iflag < 0)
1204 *info = iflag;
1205
1206 iflag = 0;
1207
1208 if (nprint > 0)
1209 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1210
1211 return (count);
1212 }
1213
1214 /*
1215 * remise a l'echelle si necessaire.
1216 */
1217
1218 if (mode != 2) {
1219 for (j = 0; j < n; j++)
1220 diag[j] = vpMath::maximum(diag[j], wa2[j]);
1221 }
1222
1223 /*
1224 * debut de la boucle la plus interne.
1225 */
1226 ratio = 0.0;
1227 while (ratio < tol0001) {
1228
1229 /*
1230 * determination du parametre de Levenberg-Marquardt.
1231 */
1232 lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4);
1233
1234 /*
1235 * stockage de la direction p et x + p. calcul de la norme de p.
1236 */
1237
1238 for (j = 0; j < n; j++) {
1239 wa1[j] = -wa1[j];
1240 wa2[j] = x[j] + wa1[j];
1241 wa3[j] = diag[j] * wa1[j];
1242 }
1243
1244 pnorm = enorm(wa3, n);
1245
1246 /*
1247 * a la premiere iteration, ajustement de la premiere limite de
1248 * l'etape.
1249 */
1250
1251 if (iter == 1)
1252 delta = vpMath::minimum(delta, pnorm);
1253
1254 /*
1255 * evaluation de la fonction en x + p et calcul de leur norme.
1256 */
1257
1258 iflag = 1;
1259 (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
1260
1261 (*nfev)++;
1262
1263 if (iflag < 0) {
1264 // termination, normal ou imposee par l'utilisateur.
1265 if (iflag < 0)
1266 *info = iflag;
1267
1268 iflag = 0;
1269
1270 if (nprint > 0)
1271 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1272
1273 return (count);
1274 }
1275
1276 fnorm1 = enorm(wa4, m);
1277
1278 /*
1279 * calcul de la reduction reelle mise a l'echelle.
1280 */
1281
1282 actred = -1.0;
1283
1284 if ((tol1 * fnorm1) < fnorm)
1285 actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
1286
1287 /*
1288 * calcul de la reduction predite mise a l'echelle et
1289 * de la derivee directionnelle mise a l'echelle.
1290 */
1291
1292 for (i = 0; i < n; i++) {
1293 wa3[i] = 0.0;
1294 l = ipvt[i];
1295 temp = wa1[l];
1296 for (j = 0; j <= i; j++)
1297 wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
1298 }
1299
1300 temp1 = enorm(wa3, n) / fnorm;
1301 temp2 = (sqrt(par) * pnorm) / fnorm;
1302 prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
1303 dirder = -((temp1 * temp1) + (temp2 * temp2));
1304
1305 /*
1306 * calcul du rapport entre la reduction reel et predit.
1307 */
1308
1309 ratio = 0.0;
1310
1311 // if (prered != 0.0)
1312 if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
1313 ratio = actred / prered;
1314
1315 /*
1316 * mise a jour de la limite de l'etape.
1317 */
1318
1319 if (ratio > tol25) {
1320 // if ((par == 0.0) || (ratio <= tol75))
1321 if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75)) {
1322 delta = pnorm / tol5;
1323 par *= tol5;
1324 }
1325 } else {
1326 if (actred >= 0.0)
1327 temp = tol5;
1328
1329 else
1330 temp = tol5 * dirder / (dirder + tol5 * actred);
1331
1332 if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
1333 temp = tol1;
1334
1335 delta = temp * vpMath::minimum(delta, (pnorm / tol1));
1336 par /= temp;
1337 }
1338
1339 /*
1340 * test pour une iteration reussie.
1341 */
1342 if (ratio >= tol0001) {
1343 /*
1344 * iteration reussie. mise a jour de x, de fvec, et de
1345 * leurs normes.
1346 */
1347
1348 for (j = 0; j < n; j++) {
1349 x[j] = wa2[j];
1350 wa2[j] = diag[j] * x[j];
1351 }
1352
1353 for (i = 0; i < m; i++)
1354 fvec[i] = wa4[i];
1355
1356 xnorm = enorm(wa2, n);
1357 fnorm = fnorm1;
1358 iter++;
1359 }
1360
1361 /*
1362 * tests pour convergence.
1363 */
1364
1365 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
1366 *info = 1;
1367
1368 if (delta <= xtol * xnorm)
1369 *info = 2;
1370
1371 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2)
1372 *info = 3;
1373
1374 if (*info != 0) {
1375 /*
1376 * termination, normal ou imposee par l'utilisateur.
1377 */
1378 if (iflag < 0)
1379 *info = iflag;
1380
1381 iflag = 0;
1382
1383 if (nprint > 0)
1384 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1385
1386 return (count);
1387 }
1388 /*
1389 * tests pour termination et
1390 * verification des tolerances.
1391 */
1392
1393 if (*nfev >= maxfev)
1394 *info = 5;
1395
1396 if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0))
1397 *info = 6;
1398
1399 if (delta <= epsmch * xnorm)
1400 *info = 7;
1401
1402 if (gnorm <= epsmch)
1403 *info = 8;
1404
1405 if (*info != 0) {
1406 /*
1407 * termination, normal ou imposee par l'utilisateur.
1408 */
1409 if (iflag < 0)
1410 *info = iflag;
1411
1412 iflag = 0;
1413
1414 if (nprint > 0)
1415 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1416
1417 return (count);
1418 }
1419 } /* fin while ratio >=tol0001 */
1420 } /*fin while 1*/
1421
1422 return 0;
1423}
1424
1425/*
1426 * PROCEDURE : lmder1
1427 *
1428 * ENTREE :
1429 * fcn Fonction qui calcule la fonction et le jacobien de la
1430 *fonction. m Nombre de fonctions. n Nombre de variables
1431 *(parametres). n <= m x Vecteur de taille "n" contenant en
1432 *entree une estimation initiale de la solution.
1433 * ldfjac Taille maximale de la matrice "fjac". ldfjac >= "m".
1434 * tol Tolerance. La terminaison de la procedure survient quand
1435 * l'algorithme estime que l'erreur relative dans la somme des
1436 * carres est au moins egal a tol ou bien que l'erreur relative
1437 * entre x et la solution est au moins egal atol.
1438 * wa Vecteur de travail de taille "lwa".
1439 * lwa Taille du vecteur "wa". wa >= 5 * n + m.
1440 *
1441 *
1442 * SORTIE :
1443 * x Vecteur de taille "n" contenant en sortie l'estimee finale
1444 * de la solution.
1445 * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x".
1446 * fjac Matrice de taille "m" x "n". La sous matrice superieure de
1447 * taille "n" x "n" de fjac contient une matrice triangulaire
1448 * superieure r dont les elements diagonaux, classe dans le sens
1449 * decroissant de leur valeur, sont de la forme :
1450 *
1451 * T T T
1452 * p * (jac * jac) * p = r * r
1453 *
1454 * Ou p est une matrice de permutation et jac est le jacobien
1455 * final calcule.
1456 * La colonne j de p est la colonne ipvt (j) (voir ci apres) de
1457 * la matrice identite. La partie trapesoidale inferieure de fjac
1458 * contient les information genere durant le calcul de r.
1459 * info Information de l'executionde la procedure. Lorsque la
1460 *procedure a termine son execution, "info" est inialisee a la valeur
1461 * (negative) de iflag. sinon elle prend les valeurs suivantes :
1462 * info = 0 : parametres en entre non valides.
1463 * info = 1 : estimation par l'algorithme que l'erreur relative
1464 * de la somme des carre est egal a tol.
1465 * info = 2 : estimation par l'algorithme que l'erreur relative
1466 * entre x et la solution est egal a tol.
1467 * info = 3 : conditions info = 1 et info = 2 tous deux requis.
1468 * info = 4 : fvec est orthogonal aux colonnes du jacobien.
1469 * info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint
1470 * 100 * (n + 1).
1471 * info = 6 : tol est trop petit. Plus moyen de reduire de la
1472 * somme des carres.
1473 * info = 7 : tol est trop petit. Plus d'amelioration possible
1474 * d' approximer la solution x.
1475 * ipvt Vecteur de taille "n". Il definit une matrice de permutation p
1476 * tel que jac * p = q * p, ou jac est le jacbien final calcule,
1477 * q est orthogonal (non socke) et r est triangulaire superieur,
1478 * avec les elements diagonaux classes en ordre decroissant de
1479 * leur valeur. La colonne j de p est ipvt[j] de la matrice
1480 *identite.
1481 *
1482 * DESCRIPTION :
1483 * La procedure minimize la somme de carre de m equation non lineaire a n
1484 * variables par une modification de l'algorithme de Levenberg - Marquardt.
1485 * Cette procedure appele la procedure generale au moindre carre lmder.
1486 *
1487 * REMARQUE :
1488 * L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et
1489 * le jacobien.
1490 * "fcn" doit etre declare dans une instruction externe a la procedure et doit
1491 * etre appele comme suit :
1492 * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
1493 **iflag)
1494 *
1495 * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
1496 * fjac n'est pas modifie.
1497 * si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac.
1498 * fvec n'est pas modifie.
1499 *
1500 * RETOUR :
1501 * En cas de succes, la valeur zero est retournee.
1502 * Sinon, la valeur -1.
1503 *
1504 */
1505int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
1506 double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa)
1507{
1508 const double factor = 100.0;
1509 unsigned int maxfev, nfev;
1510 int mode, njev, nprint;
1511 double ftol, gtol, xtol;
1512
1513 *info = 0;
1514
1515 /* verification des parametres en entree qui causent des erreurs */
1516
1517 if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < (5 * n + m))) {
1518 printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < (5 * n + m)));
1519 return (-1);
1520 }
1521
1522 /* appel a lmder */
1523
1524 maxfev = (unsigned int)(100 * (n + 1));
1525 ftol = tol;
1526 xtol = tol;
1527 gtol = 0.0;
1528 mode = 1;
1529 nprint = 0;
1530
1531 lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev,
1532 ipvt, &wa[n], &wa[2 * n], &wa[3 * n], &wa[4 * n], &wa[5 * n]);
1533
1534 if (*info == 8)
1535 *info = 4;
1536
1537 return (0);
1538}
1539
1540#undef TRUE
1541#undef FALSE
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180