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