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