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