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