39 #include <visp3/core/vpMath.h>
40 #include "vpLevenbergMarquartd.h"
42 #define SIGN(x) ((x) < 0 ? -1 : 1)
43 #define SWAP(a, b, c) \
49 #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
53 double enorm(
const double *x,
int n)
55 const double rdwarf = 3.834e-20;
56 const double rgiant = 1.304e19;
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;
65 agiant = rgiant / floatn;
67 for (i = 0; i < n; i++) {
68 double xabs = std::fabs(x[i]);
69 if ((xabs > rdwarf) && (xabs < agiant)) {
76 else if (xabs <= rdwarf) {
82 if (xabs > std::numeric_limits<double>::epsilon())
83 s3 += (xabs / x3max) * (xabs / x3max);
86 s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
96 s1 += (xabs / x1max) * (xabs / x1max);
99 s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
109 if (std::fabs(s1) <= std::numeric_limits<double>::epsilon()) {
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)));
116 norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
119 norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max);
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)
127 const double tol1 = 0.1;
134 double dwarf = DBL_MIN;
143 for (
int i = 0; i < n; i++) {
145 double *pt = MIJ(r, i, i, ldr);
147 if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
155 for (
int k = 0; k < nsing; k++) {
156 int i = nsing - 1 - k;
157 wa1[i] /= *MIJ(r, i, i, ldr);
162 for (
unsigned int j = 0; j <= (
unsigned int)jm1; j++)
163 wa1[j] -= *MIJ(r, i, j, ldr) * temp;
168 for (
int j = 0; j < n; j++) {
180 for (
int i = 0; i < n; i++)
181 wa2[i] = diag[i] * x[i];
183 dxnorm = enorm(wa2, n);
185 fp = dxnorm - *delta;
187 if (fp > tol1 * (*delta)) {
196 for (
int i = 0; i < n; i++) {
198 wa1[i] = diag[l] * (wa2[l] / dxnorm);
201 for (
int i = 0; i < n; i++) {
207 for (
unsigned int j = 0; j <= (
unsigned int)im1; j++)
208 sum += (*MIJ(r, i, j, ldr) * wa1[j]);
210 wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
213 temp = enorm(wa1, n);
214 parl = ((fp / *delta) / temp) / temp;
221 for (
int i = 0; i < n; i++) {
224 for (
int j = 0; j <= i; j++)
225 sum += *MIJ(r, i, j, ldr) * qtb[j];
228 wa1[i] = sum / diag[l];
231 double gnorm = enorm(wa1, n);
232 double paru = gnorm / *delta;
235 if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
247 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon())
248 *par = gnorm / dxnorm;
262 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon()) {
263 const double tol001 = 0.001;
269 for (
int i = 0; i < n; i++)
270 wa1[i] = temp * diag[i];
272 qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
274 for (
int i = 0; i < n; i++)
275 wa2[i] = diag[i] * x[i];
277 dxnorm = enorm(wa2, n);
279 fp = dxnorm - *delta;
290 if ((std::fabs(fp) <= tol1 * (*delta)) ||
291 ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp) && (temp < 0.0)) ||
306 for (
int i = 0; i < n; i++) {
308 wa1[i] = diag[l] * (wa2[l] / dxnorm);
311 for (
unsigned int i = 0; i < (
unsigned int)n; i++) {
312 wa1[i] = wa1[i] / sdiag[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);
321 temp = enorm(wa1, n);
322 double parc = ((fp / *delta) / temp) / temp;
350 double pythag(
double a,
double b)
352 double pyth, p, r, t;
357 if (std::fabs(p) <= std::numeric_limits<double>::epsilon()) {
362 r = (std::min<double>(std::fabs(a), std::fabs(b)) / p) * (std::min<double>(std::fabs(a), std::fabs(b)) / p);
366 while (std::fabs(t - 4.0) < std::fabs(
vpMath::maximum(t, 4.0)) * std::numeric_limits<double>::epsilon()) {
368 double u = 1.0 + 2.0 * s;
370 r *= (s / u) * (s / u);
378 int qrfac(
int m,
int n,
double *a,
int lda,
int *pivot,
int *ipvt,
int ,
double *rdiag,
double *acnorm,
381 const double tolerance = 0.05;
383 int i, j, ip1, k, kmax, minmn;
385 double sum, temp, tmp;
390 epsmch = std::numeric_limits<double>::epsilon();
396 for (i = 0; i < m; i++) {
397 acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
398 rdiag[i] = acnorm[i];
408 for (i = 0; i < minmn; i++) {
415 for (k = i; k < m; k++) {
416 if (rdiag[k] > rdiag[kmax])
421 for (j = 0; j < n; j++)
422 SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp);
424 rdiag[kmax] = rdiag[i];
427 SWAP(ipvt[i], ipvt[kmax], k);
435 double ajnorm = enorm(MIJ(a, i, i, lda), n - i);
438 if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon()) {
439 if (*MIJ(a, i, i, lda) < 0.0)
442 for (j = i; j < n; j++)
443 *MIJ(a, i, j, lda) /= ajnorm;
444 *MIJ(a, i, i, lda) += 1.0;
453 for (k = ip1; k < m; k++) {
455 for (j = i; j < n; j++)
456 sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
458 temp = sum / *MIJ(a, i, i, lda);
460 for (j = i; j < n; j++)
461 *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
464 if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon())) {
465 temp = *MIJ(a, k, i, lda) / rdiag[k];
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));
483 int qrsolv(
int n,
double *r,
int ldr,
int *ipvt,
double *diag,
double *qtb,
double *x,
double *sdiag,
double *wa)
487 double cosi, cotg, qtbpj, sinu, tg, temp;
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);
498 x[i] = *MIJ(r, i, i, ldr);
507 for (i = 0; i < n; i++) {
516 if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon()) {
517 for (k = i; k < n; k++)
531 for (k = i; k < n; k++) {
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));
546 cotg = *MIJ(r, k, k, ldr) / sdiag[k];
547 sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
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;
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;
583 sdiag[i] = *MIJ(r, i, i, ldr);
584 *MIJ(r, i, i, ldr) = x[i];
593 for (i = 0; i < n; i++) {
595 if ((std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
603 for (k = 0; k < nsing; k++) {
609 for (j = jp1; j < nsing; j++)
610 sum += *MIJ(r, i, j, ldr) * wa[j];
612 wa[i] = (wa[i] - sum) / sdiag[i];
619 for (j = 0; j < n; j++) {
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)
631 const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
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;
642 epsmch = std::numeric_limits<double>::epsilon();
667 if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0) || (maxfev == 0) ||
678 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
684 for (j = 0; j < n; j++) {
685 if (diag[j] <= 0.0) {
695 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
708 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
722 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
727 fnorm = enorm(fvec, m);
740 while (count < (
int)maxfev) {
748 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
762 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
772 if ((iter - 1) % nprint == 0)
773 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
785 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
794 qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3);
803 for (j = 0; j < n; j++) {
806 if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
817 for (j = 0; j < n; j++)
818 wa3[j] = diag[j] * x[j];
820 xnorm = enorm(wa3, n);
821 delta = factor * xnorm;
824 if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
832 for (i = 0; i < m; i++)
835 for (i = 0; i < n; i++) {
836 double *pt = MIJ(fjac, i, i, ldfjac);
838 if (std::fabs(*pt) > std::numeric_limits<double>::epsilon()) {
841 for (j = i; j < m; j++)
842 sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
844 temp = -sum / *MIJ(fjac, i, i, ldfjac);
846 for (j = i; j < m; j++)
847 wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
850 *MIJ(fjac, i, i, ldfjac) = wa1[i];
861 if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon()) {
862 for (i = 0; i < n; i++) {
865 if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon()) {
867 for (j = 0; j <= i; j++)
868 sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
892 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
902 for (j = 0; j < n; j++)
910 while (ratio < tol0001) {
915 lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4);
921 for (j = 0; j < n; j++) {
923 wa2[j] = x[j] + wa1[j];
924 wa3[j] = diag[j] * wa1[j];
927 pnorm = enorm(wa3, n);
942 (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
954 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
959 fnorm1 = enorm(wa4, m);
967 if ((tol1 * fnorm1) < fnorm)
968 actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
975 for (i = 0; i < n; i++) {
979 for (j = 0; j <= i; j++)
980 wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
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));
995 if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
996 ratio = actred / prered;
1002 if (ratio > tol25) {
1004 if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75)) {
1005 delta = pnorm / tol5;
1014 temp = tol5 * dirder / (dirder + tol5 * actred);
1016 if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
1026 if (ratio >= tol0001) {
1032 for (j = 0; j < n; j++) {
1034 wa2[j] = diag[j] * x[j];
1037 for (i = 0; i < m; i++)
1040 xnorm = enorm(wa2, n);
1049 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
1052 if (delta <= xtol * xnorm)
1055 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2)
1068 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1077 if (*nfev >= maxfev)
1080 if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0))
1083 if (delta <= epsmch * xnorm)
1086 if (gnorm <= epsmch)
1099 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
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)
1112 const double factor = 100.0;
1113 unsigned int maxfev, nfev;
1114 int mode, njev, nprint;
1115 double ftol, gtol, xtol;
1121 if ( (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)));
1128 maxfev = (
unsigned int)(100 * (n + 1));
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]);
static Type maximum(const Type &a, const Type &b)
static Type minimum(const Type &a, const Type &b)