Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpLevenbergMarquartd.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Levenberg Marquartd.
32  */
33 
34 #include <algorithm> // (std::min)
35 #include <cmath> // std::fabs
36 #include <iostream>
37 #include <limits> // numeric_limits
38 
39 #include <visp3/core/vpMath.h>
40 #include "vpLevenbergMarquartd.h"
41 
42 #define SIGN(x) ((x) < 0 ? -1 : 1)
43 #define SWAP(a, b, c) \
44  { \
45  (c) = (a); \
46  (a) = (b); \
47  (b) = (c); \
48  }
49 #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
50 #define TRUE 1
51 #define FALSE 0
52 
53 double enorm(const double *x, int n)
54 {
55  const double rdwarf = 3.834e-20;
56  const double rgiant = 1.304e19;
57 
58  int i;
59  double agiant, floatn;
60  double norm_eucl = 0.0;
61  double s1 = 0.0, s2 = 0.0, s3 = 0.0;
62  double x1max = 0.0, x3max = 0.0;
63 
64  floatn = (double)n;
65  agiant = rgiant / floatn;
66 
67  for (i = 0; i < n; i++) {
68  double xabs = std::fabs(x[i]);
69  if ((xabs > rdwarf) && (xabs < agiant)) {
70  /*
71  * somme pour elements intemediaires.
72  */
73  s2 += xabs * xabs;
74  }
75 
76  else if (xabs <= rdwarf) {
77  /*
78  * somme pour elements petits.
79  */
80  if (xabs <= x3max) {
81  // if (xabs != 0.0)
82  if (xabs > std::numeric_limits<double>::epsilon())
83  s3 += (xabs / x3max) * (xabs / x3max);
84  }
85  else {
86  s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
87  x3max = xabs;
88  }
89  }
90 
91  else {
92  /*
93  * somme pour elements grand.
94  */
95  if (xabs <= x1max) {
96  s1 += (xabs / x1max) * (xabs / x1max);
97  }
98  else {
99  s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
100  x1max = xabs;
101  }
102  }
103  }
104 
105  /*
106  * calcul de la norme.
107  */
108  // if (s1 == 0.0)
109  if (std::fabs(s1) <= std::numeric_limits<double>::epsilon()) {
110  // if (s2 == 0.0)
111  if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
112  norm_eucl = x3max * sqrt(s3);
113  else if (s2 >= x3max)
114  norm_eucl = sqrt(s2 * (1.0 + (x3max / s2) * (x3max * s3)));
115  else /*if (s2 < x3max)*/
116  norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
117  }
118  else
119  norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max);
120 
121  return (norm_eucl);
122 }
123 
124 int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *delta, double *par, double *x,
125  double *sdiag, double *wa1, double *wa2)
126 {
127  const double tol1 = 0.1; /* tolerance a 0.1 */
128 
129  int l;
130  unsigned int iter; /* compteur d'iteration */
131  int nsing; /* nombre de singularite de la matrice */
132  double dxnorm, fp;
133  double temp;
134  double dwarf = DBL_MIN; /* plus petite amplitude positive */
135 
136  /*
137  * calcul et stockage dans "x" de la direction de Gauss-Newton. Si
138  * le jacobien a une rangee de moins, on a une solution au moindre
139  * carres.
140  */
141  nsing = n;
142 
143  for (int i = 0; i < n; i++) {
144  wa1[i] = qtb[i];
145  double *pt = MIJ(r, i, i, ldr);
146  // if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
147  if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
148  nsing = i - 1;
149 
150  if (nsing < n)
151  wa1[i] = 0.0;
152  }
153 
154  if (nsing >= 0) {
155  for (int k = 0; k < nsing; k++) {
156  int i = nsing - 1 - k;
157  wa1[i] /= *MIJ(r, i, i, ldr);
158  temp = wa1[i];
159  int jm1 = i - 1;
160 
161  if (jm1 >= 0) {
162  for (unsigned int j = 0; j <= (unsigned int)jm1; j++)
163  wa1[j] -= *MIJ(r, i, j, ldr) * temp;
164  }
165  }
166  }
167 
168  for (int j = 0; j < n; j++) {
169  l = ipvt[j];
170  x[l] = wa1[j];
171  }
172 
173  /*
174  * initialisation du compteur d'iteration.
175  * evaluation de la fonction a l'origine, et test
176  * d'acceptation de la direction de Gauss-Newton.
177  */
178  iter = 0;
179 
180  for (int i = 0; i < n; i++)
181  wa2[i] = diag[i] * x[i];
182 
183  dxnorm = enorm(wa2, n);
184 
185  fp = dxnorm - *delta;
186 
187  if (fp > tol1 * (*delta)) {
188  /*
189  * Si le jacobien n'a pas de rangee deficiente,l'etape de
190  * Newton fournit une limite inferieure, parl pour le
191  * zero de la fonction. Sinon cette limite vaut 0.0.
192  */
193  double parl = 0.0;
194 
195  if (nsing >= n) {
196  for (int i = 0; i < n; i++) {
197  l = ipvt[i];
198  wa1[i] = diag[l] * (wa2[l] / dxnorm);
199  }
200 
201  for (int i = 0; i < n; i++) {
202  long im1;
203  double sum = 0.0;
204  im1 = (i - 1L);
205 
206  if (im1 >= 0) {
207  for (unsigned int j = 0; j <= (unsigned int)im1; j++)
208  sum += (*MIJ(r, i, j, ldr) * wa1[j]);
209  }
210  wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
211  }
212 
213  temp = enorm(wa1, n);
214  parl = ((fp / *delta) / temp) / temp;
215  }
216 
217  /*
218  * calcul d'une limite superieure, paru, pour le zero de la
219  * fonction.
220  */
221  for (int i = 0; i < n; i++) {
222  double sum = 0.0;
223 
224  for (int j = 0; j <= i; j++)
225  sum += *MIJ(r, i, j, ldr) * qtb[j];
226 
227  l = ipvt[i];
228  wa1[i] = sum / diag[l];
229  }
230 
231  double gnorm = enorm(wa1, n);
232  double paru = gnorm / *delta;
233 
234  // if (paru == 0.0)
235  if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
236  paru = dwarf / vpMath::minimum(*delta, tol1);
237 
238  /*
239  * Si "par" en entree tombe hors de l'intervalle (parl,paru),
240  * on le prend proche du point final.
241  */
242 
243  *par = vpMath::maximum(*par, parl);
244  *par = vpMath::maximum(*par, paru);
245 
246  // if (*par == 0.0)
247  if (std::fabs(*par) <= std::numeric_limits<double>::epsilon())
248  *par = gnorm / dxnorm;
249 
250  /*
251  * debut d'une iteration.
252  */
253  for (;;) // iter >= 0)
254  {
255  iter++;
256 
257  /*
258  * evaluation de la fonction a la valeur courant
259  * de "par".
260  */
261  // if (*par == 0.0)
262  if (std::fabs(*par) <= std::numeric_limits<double>::epsilon()) {
263  const double tol001 = 0.001; /* tolerance a 0.001 */
264  *par = vpMath::maximum(dwarf, (tol001 * paru));
265  }
266 
267  temp = sqrt(*par);
268 
269  for (int i = 0; i < n; i++)
270  wa1[i] = temp * diag[i];
271 
272  qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
273 
274  for (int i = 0; i < n; i++)
275  wa2[i] = diag[i] * x[i];
276 
277  dxnorm = enorm(wa2, n);
278  temp = fp;
279  fp = dxnorm - *delta;
280 
281  /*
282  * si la fonction est assez petite, acceptation de
283  * la valeur courant de "par". de plus, test des cas
284  * ou parl est nul et ou le nombre d'iteration a
285  * atteint 10.
286  */
287  // if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <=
288  // temp)
289  // && (temp < 0.0)) || (iter == 10))
290  if ((std::fabs(fp) <= tol1 * (*delta)) ||
291  ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp) && (temp < 0.0)) ||
292  (iter == 10)) {
293  // terminaison.
294 
295  // Remove the two next lines since this is a dead code
296  /* if (iter == 0)
297  *par = 0.0; */
298 
299  return (0);
300  }
301 
302  /*
303  * calcul de la correction de Newton.
304  */
305 
306  for (int i = 0; i < n; i++) {
307  l = ipvt[i];
308  wa1[i] = diag[l] * (wa2[l] / dxnorm);
309  }
310 
311  for (unsigned int i = 0; i < (unsigned int)n; i++) {
312  wa1[i] = wa1[i] / sdiag[i];
313  temp = wa1[i];
314  unsigned int jp1 = i + 1;
315  if ((unsigned int)n >= jp1) {
316  for (unsigned int j = jp1; j < (unsigned int)n; j++)
317  wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
318  }
319  }
320 
321  temp = enorm(wa1, n);
322  double parc = ((fp / *delta) / temp) / temp;
323 
324  /*
325  * selon le signe de la fonction, mise a jour
326  * de parl ou paru.
327  */
328  if (fp > 0.0)
329  parl = vpMath::maximum(parl, *par);
330 
331  if (fp < 0.0)
332  paru = vpMath::minimum(paru, *par);
333 
334  /*
335  * calcul d'une estimee ameliree de "par".
336  */
337  *par = vpMath::maximum(parl, (*par + parc));
338  } /* fin boucle sur iter */
339  } /* fin fp > tol1 * delta */
340 
341  /*
342  * terminaison.
343  */
344  if (iter == 0)
345  *par = 0.0;
346 
347  return (0);
348 }
349 
350 double pythag(double a, double b)
351 {
352  double pyth, p, r, t;
353 
354  p = vpMath::maximum(std::fabs(a), std::fabs(b));
355 
356  // if (p == 0.0)
357  if (std::fabs(p) <= std::numeric_limits<double>::epsilon()) {
358  pyth = p;
359  return (pyth);
360  }
361 
362  r = (std::min<double>(std::fabs(a), std::fabs(b)) / p) * (std::min<double>(std::fabs(a), std::fabs(b)) / p);
363  t = 4.0 + r;
364 
365  // while (t != 4.0)
366  while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits<double>::epsilon()) {
367  double s = r / t;
368  double u = 1.0 + 2.0 * s;
369  p *= u;
370  r *= (s / u) * (s / u);
371  t = 4.0 + r;
372  }
373 
374  pyth = p;
375  return (pyth);
376 }
377 
378 int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt */, double *rdiag, double *acnorm,
379  double *wa)
380 {
381  const double tolerance = 0.05;
382 
383  int i, j, ip1, k, kmax, minmn;
384  double epsmch;
385  double sum, temp, tmp;
386 
387  /*
388  * epsmch est la precision machine.
389  */
390  epsmch = std::numeric_limits<double>::epsilon();
391 
392  /*
393  * calcul des normes initiales des lignes et initialisation
394  * de plusieurs tableaux.
395  */
396  for (i = 0; i < m; i++) {
397  acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
398  rdiag[i] = acnorm[i];
399  wa[i] = rdiag[i];
400 
401  if (pivot)
402  ipvt[i] = i;
403  }
404  /*
405  * reduction de "a" en "r" avec les transformations de Householder.
406  */
407  minmn = vpMath::minimum(m, n);
408  for (i = 0; i < minmn; i++) {
409  if (pivot) {
410  /*
411  * met la ligne de plus grande norme en position
412  * de pivot.
413  */
414  kmax = i;
415  for (k = i; k < m; k++) {
416  if (rdiag[k] > rdiag[kmax])
417  kmax = k;
418  }
419 
420  if (kmax != i) {
421  for (j = 0; j < n; j++)
422  SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp);
423 
424  rdiag[kmax] = rdiag[i];
425  wa[kmax] = wa[i];
426 
427  SWAP(ipvt[i], ipvt[kmax], k);
428  }
429  }
430 
431  /*
432  * calcul de al transformationde Householder afin de reduire
433  * la jeme ligne de "a" a un multiple du jeme vecteur unite.
434  */
435  double ajnorm = enorm(MIJ(a, i, i, lda), n - i);
436 
437  // if (ajnorm != 0.0)
438  if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon()) {
439  if (*MIJ(a, i, i, lda) < 0.0)
440  ajnorm = -ajnorm;
441 
442  for (j = i; j < n; j++)
443  *MIJ(a, i, j, lda) /= ajnorm;
444  *MIJ(a, i, i, lda) += 1.0;
445 
446  /*
447  * application de la transformation aux lignes
448  * restantes et mise a jour des normes.
449  */
450  ip1 = i + 1;
451 
452  if (m >= ip1) {
453  for (k = ip1; k < m; k++) {
454  sum = 0.0;
455  for (j = i; j < n; j++)
456  sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
457 
458  temp = sum / *MIJ(a, i, i, lda);
459 
460  for (j = i; j < n; j++)
461  *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
462 
463  // if (pivot && rdiag[k] != 0.0)
464  if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon())) {
465  temp = *MIJ(a, k, i, lda) / rdiag[k];
466  rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
467 
468  if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch) {
469  rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - (int)i));
470  wa[k] = rdiag[k];
471  }
472  }
473  } /* fin boucle for k */
474  }
475 
476  } /* fin if (ajnorm) */
477 
478  rdiag[i] = -ajnorm;
479  } /* fin for (i = 0; i < minmn; i++) */
480  return (0);
481 }
482 
483 int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa)
484 {
485  int i, j, k, kp1, l; /* compteur de boucle */
486  int nsing;
487  double cosi, cotg, qtbpj, sinu, tg, temp;
488 
489  /*
490  * copie de r et (q transpose) * b afin de preserver l'entree
491  * et initialisation de "s". En particulier, sauvegarde des elements
492  * diagonaux de "r" dans "x".
493  */
494  for (i = 0; i < n; i++) {
495  for (j = i; j < n; j++)
496  *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
497 
498  x[i] = *MIJ(r, i, i, ldr);
499  wa[i] = qtb[i];
500  }
501 
502  /*
503  * Elimination de la matrice diagonale "d" en utlisant une rotation
504  * connue.
505  */
506 
507  for (i = 0; i < n; i++) {
508  /*
509  * preparation de la colonne de d a eliminer, reperage de
510  * l'element diagonal par utilisation de p de la
511  * factorisation qr.
512  */
513  l = ipvt[i];
514 
515  // if (diag[l] != 0.0)
516  if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon()) {
517  for (k = i; k < n; k++)
518  sdiag[k] = 0.0;
519 
520  sdiag[i] = diag[l];
521 
522  /*
523  * Les transformations qui eliminent la colonne de d
524  * modifient seulement qu'un seul element de
525  * (q transpose)*b avant les n premiers elements
526  * lesquels sont inialement nuls.
527  */
528 
529  qtbpj = 0.0;
530 
531  for (k = i; k < n; k++) {
532  /*
533  * determination d'une rotation qui elimine
534  * les elements appropriees dans la colonne
535  * courante de d.
536  */
537 
538  // if (sdiag[k] != 0.0)
539  if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon()) {
540  if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k])) {
541  tg = sdiag[k] / *MIJ(r, k, k, ldr);
542  cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
543  sinu = cosi * tg;
544  }
545  else {
546  cotg = *MIJ(r, k, k, ldr) / sdiag[k];
547  sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
548  cosi = sinu * cotg;
549  }
550 
551  /*
552  * calcul des elements de la diagonale modifiee
553  * de r et des elements modifies de
554  * ((q transpose)*b,0).
555  */
556  *MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k];
557  temp = cosi * wa[k] + sinu * qtbpj;
558  qtbpj = -sinu * wa[k] + cosi * qtbpj;
559  wa[k] = temp;
560 
561  /*
562  * accumulation des transformations dans
563  * les lignes de s.
564  */
565 
566  kp1 = k + 1;
567 
568  if (n >= kp1) {
569  for (j = kp1; j < n; j++) {
570  temp = cosi * *MIJ(r, k, j, ldr) + sinu * sdiag[j];
571  sdiag[j] = -sinu * *MIJ(r, k, j, ldr) + cosi * sdiag[j];
572  *MIJ(r, k, j, ldr) = temp;
573  }
574  }
575  } /* fin if diag[] !=0 */
576  } /* fin boucle for k -> n */
577  } /* fin if diag =0 */
578 
579  /*
580  * stokage de l'element diagonal de s et restauration de
581  * l'element diagonal correspondant de r.
582  */
583  sdiag[i] = *MIJ(r, i, i, ldr);
584  *MIJ(r, i, i, ldr) = x[i];
585  } /* fin boucle for j -> n */
586 
587  /*
588  * resolution du systeme triangulaire pour z. Si le systeme est
589  * singulier, on obtient une solution au moindres carres.
590  */
591  nsing = n;
592 
593  for (i = 0; i < n; i++) {
594  // if (sdiag[i] == 0.0 && nsing == n)
595  if ((std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
596  nsing = i - 1;
597 
598  if (nsing < n)
599  wa[i] = 0.0;
600  }
601 
602  if (nsing >= 0) {
603  for (k = 0; k < nsing; k++) {
604  i = nsing - 1 - k;
605  double sum = 0.0;
606  int jp1 = i + 1;
607 
608  if (nsing >= jp1) {
609  for (j = jp1; j < nsing; j++)
610  sum += *MIJ(r, i, j, ldr) * wa[j];
611  }
612  wa[i] = (wa[i] - sum) / sdiag[i];
613  }
614  }
615  /*
616  * permutation arriere des composants de z et des componants de x.
617  */
618 
619  for (j = 0; j < n; j++) {
620  l = ipvt[j];
621  x[l] = wa[j];
622  }
623  return (0);
624 }
625 
626 int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
627  double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev,
628  double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt,
629  double *qtf, double *wa1, double *wa2, double *wa3, double *wa4)
630 {
631  const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
632  int oncol = TRUE;
633  int iflag, iter;
634  int count = 0;
635  int i, j, l;
636  double actred, delta, dirder, epsmch, fnorm, fnorm1;
637  double ratio = std::numeric_limits<double>::epsilon();
638  double par, pnorm, prered;
639  double sum, temp, temp1, temp2, xnorm = 0.0;
640 
641  /* epsmch est la precision machine. */
642  epsmch = std::numeric_limits<double>::epsilon();
643 
644  *info = 0;
645  iflag = 0;
646  *nfev = 0;
647  *njev = 0;
648 
649  /* verification des parametres d'entree. */
650 
651  /*if (n <= 0)
652  return 0;*/
653  if (m < n)
654  return 0;
655  if (ldfjac < m)
656  return 0;
657  if (ftol < 0.0)
658  return 0;
659  if (xtol < 0.0)
660  return 0;
661  if (gtol < 0.0)
662  return 0;
663  if (maxfev == 0)
664  return 0;
665  if (factor <= 0.0)
666  return 0;
667  if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0) || (maxfev == 0) ||
668  (factor <= 0.0)) {
669  /*
670  * termination, normal ou imposee par l'utilisateur.
671  */
672  if (iflag < 0)
673  *info = iflag;
674 
675  iflag = 0;
676 
677  if (nprint > 0)
678  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
679 
680  return (count);
681  }
682 
683  if (mode == 2) {
684  for (j = 0; j < n; j++) {
685  if (diag[j] <= 0.0) {
686  /*
687  * termination, normal ou imposee par l'utilisateur.
688  */
689  if (iflag < 0)
690  *info = iflag;
691 
692  iflag = 0;
693 
694  if (nprint > 0)
695  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
696 
697  return (count);
698  }
699  }
700  }
701 
702  /*
703  * evaluation de la fonction au point de depart
704  * et calcul de sa norme.
705  */
706  iflag = 1;
707 
708  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
709 
710  *nfev = 1;
711 
712  if (iflag < 0) {
713  /*
714  * termination, normal ou imposee par l'utilisateur.
715  */
716  if (iflag < 0)
717  *info = iflag;
718 
719  iflag = 0;
720 
721  if (nprint > 0)
722  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
723 
724  return (count);
725  }
726 
727  fnorm = enorm(fvec, m);
728 
729  /*
730  * initialisation du parametre de Levenberg-Marquardt
731  * et du conteur d'iteration.
732  */
733 
734  par = 0.0;
735  iter = 1;
736 
737  /*
738  * debut de la boucle la plus externe.
739  */
740  while (count < (int)maxfev) {
741  count++;
742  /*
743  * calcul de la matrice jacobienne.
744  */
745 
746  iflag = 2;
747 
748  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
749 
750  (*njev)++;
751 
752  if (iflag < 0) {
753  /*
754  * termination, normal ou imposee par l'utilisateur.
755  */
756  if (iflag < 0)
757  *info = iflag;
758 
759  iflag = 0;
760 
761  if (nprint > 0)
762  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
763 
764  return (count);
765  }
766 
767  /*
768  * si demandee, appel de fcn pour impression des iterees.
769  */
770  if (nprint > 0) {
771  iflag = 0;
772  if ((iter - 1) % nprint == 0)
773  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
774 
775  if (iflag < 0) {
776  /*
777  * termination, normal ou imposee par l'utilisateur.
778  */
779  if (iflag < 0)
780  *info = iflag;
781 
782  iflag = 0;
783 
784  if (nprint > 0)
785  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
786 
787  return (count);
788  }
789  }
790 
791  /*
792  * calcul de la factorisation qr du jacobien.
793  */
794  qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3);
795 
796  /*
797  * a la premiere iteration et si mode est 1, mise a l'echelle
798  * en accord avec les normes des colonnes du jacobien initial.
799  */
800 
801  if (iter == 1) {
802  if (mode != 2) {
803  for (j = 0; j < n; j++) {
804  diag[j] = wa2[j];
805  // if (wa2[j] == 0.0)
806  if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
807  diag[j] = 1.0;
808  }
809  }
810 
811  /*
812  * a la premiere iteration, calcul de la norme de x mis
813  * a l'echelle et initialisation de la limite delta de
814  * l'etape.
815  */
816 
817  for (j = 0; j < n; j++)
818  wa3[j] = diag[j] * x[j];
819 
820  xnorm = enorm(wa3, n);
821  delta = factor * xnorm;
822 
823  // if (delta == 0.0)
824  if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
825  delta = factor;
826  }
827 
828  /*
829  * formation de (q transpose) * fvec et stockage des n premiers
830  * composants dans qtf.
831  */
832  for (i = 0; i < m; i++)
833  wa4[i] = fvec[i];
834 
835  for (i = 0; i < n; i++) {
836  double *pt = MIJ(fjac, i, i, ldfjac);
837  // if (*MIJ(fjac, i, i, ldfjac) != 0.0)
838  if (std::fabs(*pt) > std::numeric_limits<double>::epsilon()) {
839  sum = 0.0;
840 
841  for (j = i; j < m; j++)
842  sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
843 
844  temp = -sum / *MIJ(fjac, i, i, ldfjac);
845 
846  for (j = i; j < m; j++)
847  wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
848  }
849 
850  *MIJ(fjac, i, i, ldfjac) = wa1[i];
851  qtf[i] = wa4[i];
852  }
853 
854  /*
855  * calcul de la norme du gradient mis a l'echelle.
856  */
857 
858  double gnorm = 0.0;
859 
860  // if (fnorm != 0.0)
861  if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon()) {
862  for (i = 0; i < n; i++) {
863  l = ipvt[i];
864  // if (wa2[l] != 0.0)
865  if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon()) {
866  sum = 0.0;
867  for (j = 0; j <= i; j++)
868  sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
869 
870  gnorm = vpMath::maximum(gnorm, std::fabs(sum / wa2[l]));
871  }
872  }
873  }
874 
875  /*
876  * test pour la convergence de la norme du gradient .
877  */
878 
879  if (gnorm <= gtol)
880  *info = 4;
881 
882  if (*info != 0) {
883  /*
884  * termination, normal ou imposee par l'utilisateur.
885  */
886  if (iflag < 0)
887  *info = iflag;
888 
889  iflag = 0;
890 
891  if (nprint > 0)
892  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
893 
894  return (count);
895  }
896 
897  /*
898  * remise a l'echelle si necessaire.
899  */
900 
901  if (mode != 2) {
902  for (j = 0; j < n; j++)
903  diag[j] = vpMath::maximum(diag[j], wa2[j]);
904  }
905 
906  /*
907  * debut de la boucle la plus interne.
908  */
909  ratio = 0.0;
910  while (ratio < tol0001) {
911 
912  /*
913  * determination du parametre de Levenberg-Marquardt.
914  */
915  lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4);
916 
917  /*
918  * stockage de la direction p et x + p. calcul de la norme de p.
919  */
920 
921  for (j = 0; j < n; j++) {
922  wa1[j] = -wa1[j];
923  wa2[j] = x[j] + wa1[j];
924  wa3[j] = diag[j] * wa1[j];
925  }
926 
927  pnorm = enorm(wa3, n);
928 
929  /*
930  * a la premiere iteration, ajustement de la premiere limite de
931  * l'etape.
932  */
933 
934  if (iter == 1)
935  delta = vpMath::minimum(delta, pnorm);
936 
937  /*
938  * evaluation de la fonction en x + p et calcul de leur norme.
939  */
940 
941  iflag = 1;
942  (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
943 
944  (*nfev)++;
945 
946  if (iflag < 0) {
947  // termination, normal ou imposee par l'utilisateur.
948  if (iflag < 0)
949  *info = iflag;
950 
951  iflag = 0;
952 
953  if (nprint > 0)
954  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
955 
956  return (count);
957  }
958 
959  fnorm1 = enorm(wa4, m);
960 
961  /*
962  * calcul de la reduction reelle mise a l'echelle.
963  */
964 
965  actred = -1.0;
966 
967  if ((tol1 * fnorm1) < fnorm)
968  actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
969 
970  /*
971  * calcul de la reduction predite mise a l'echelle et
972  * de la derivee directionnelle mise a l'echelle.
973  */
974 
975  for (i = 0; i < n; i++) {
976  wa3[i] = 0.0;
977  l = ipvt[i];
978  temp = wa1[l];
979  for (j = 0; j <= i; j++)
980  wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
981  }
982 
983  temp1 = enorm(wa3, n) / fnorm;
984  temp2 = (sqrt(par) * pnorm) / fnorm;
985  prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
986  dirder = -((temp1 * temp1) + (temp2 * temp2));
987 
988  /*
989  * calcul du rapport entre la reduction reel et predit.
990  */
991 
992  ratio = 0.0;
993 
994  // if (prered != 0.0)
995  if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
996  ratio = actred / prered;
997 
998  /*
999  * mise a jour de la limite de l'etape.
1000  */
1001 
1002  if (ratio > tol25) {
1003  // if ((par == 0.0) || (ratio <= tol75))
1004  if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75)) {
1005  delta = pnorm / tol5;
1006  par *= tol5;
1007  }
1008  }
1009  else {
1010  if (actred >= 0.0)
1011  temp = tol5;
1012 
1013  else
1014  temp = tol5 * dirder / (dirder + tol5 * actred);
1015 
1016  if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
1017  temp = tol1;
1018 
1019  delta = temp * vpMath::minimum(delta, (pnorm / tol1));
1020  par /= temp;
1021  }
1022 
1023  /*
1024  * test pour une iteration reussie.
1025  */
1026  if (ratio >= tol0001) {
1027  /*
1028  * iteration reussie. mise a jour de x, de fvec, et de
1029  * leurs normes.
1030  */
1031 
1032  for (j = 0; j < n; j++) {
1033  x[j] = wa2[j];
1034  wa2[j] = diag[j] * x[j];
1035  }
1036 
1037  for (i = 0; i < m; i++)
1038  fvec[i] = wa4[i];
1039 
1040  xnorm = enorm(wa2, n);
1041  fnorm = fnorm1;
1042  iter++;
1043  }
1044 
1045  /*
1046  * tests pour convergence.
1047  */
1048 
1049  if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
1050  *info = 1;
1051 
1052  if (delta <= xtol * xnorm)
1053  *info = 2;
1054 
1055  if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2)
1056  *info = 3;
1057 
1058  if (*info != 0) {
1059  /*
1060  * termination, normal ou imposee par l'utilisateur.
1061  */
1062  if (iflag < 0)
1063  *info = iflag;
1064 
1065  iflag = 0;
1066 
1067  if (nprint > 0)
1068  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1069 
1070  return (count);
1071  }
1072  /*
1073  * tests pour termination et
1074  * verification des tolerances.
1075  */
1076 
1077  if (*nfev >= maxfev)
1078  *info = 5;
1079 
1080  if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0))
1081  *info = 6;
1082 
1083  if (delta <= epsmch * xnorm)
1084  *info = 7;
1085 
1086  if (gnorm <= epsmch)
1087  *info = 8;
1088 
1089  if (*info != 0) {
1090  /*
1091  * termination, normal ou imposee par l'utilisateur.
1092  */
1093  if (iflag < 0)
1094  *info = iflag;
1095 
1096  iflag = 0;
1097 
1098  if (nprint > 0)
1099  (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1100 
1101  return (count);
1102  }
1103  } /* fin while ratio >=tol0001 */
1104  } /*fin while 1*/
1105 
1106  return 0;
1107 }
1108 
1109 int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
1110  double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa)
1111 {
1112  const double factor = 100.0;
1113  unsigned int maxfev, nfev;
1114  int mode, njev, nprint;
1115  double ftol, gtol, xtol;
1116 
1117  *info = 0;
1118 
1119  /* verification des parametres en entree qui causent des erreurs */
1120 
1121  if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < (5 * n + m))) {
1122  printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < (5 * n + m)));
1123  return (-1);
1124  }
1125 
1126  /* appel a lmder */
1127 
1128  maxfev = (unsigned int)(100 * (n + 1));
1129  ftol = tol;
1130  xtol = tol;
1131  gtol = 0.0;
1132  mode = 1;
1133  nprint = 0;
1134 
1135  lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev,
1136  ipvt, &wa[n], &wa[2 * n], &wa[3 * n], &wa[4 * n], &wa[5 * n]);
1137 
1138  if (*info == 8)
1139  *info = 4;
1140 
1141  return (0);
1142 }
1143 
1144 #undef TRUE
1145 #undef FALSE
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260