39#include <visp3/core/vpMath.h>
40#include <visp3/vision/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))
80double enorm(
const double *x,
int n)
82 const double rdwarf = 3.834e-20;
83 const double rgiant = 1.304e19;
86 double agiant, floatn;
87 double norm_eucl = 0.0;
88 double s1 = 0.0, s2 = 0.0, s3 = 0.0;
89 double x1max = 0.0, x3max = 0.0;
92 agiant = rgiant / floatn;
94 for (i = 0; i < n; i++) {
95 double xabs = std::fabs(x[i]);
96 if ((xabs > rdwarf) && (xabs < agiant)) {
103 else if (xabs <= rdwarf) {
109 if (xabs > std::numeric_limits<double>::epsilon())
110 s3 += (xabs / x3max) * (xabs / x3max);
112 s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
122 s1 += (xabs / x1max) * (xabs / x1max);
124 s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
134 if (std::fabs(s1) <= std::numeric_limits<double>::epsilon()) {
136 if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
137 norm_eucl = x3max * sqrt(s3);
138 else if (s2 >= x3max)
139 norm_eucl = sqrt(s2 * (1.0 + (x3max / s2) * (x3max * s3)));
141 norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
143 norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max);
220int lmpar(
int n,
double *r,
int ldr,
int *ipvt,
double *diag,
double *qtb,
double *delta,
double *par,
double *x,
221 double *sdiag,
double *wa1,
double *wa2)
223 const double tol1 = 0.1;
230 double dwarf = DBL_MIN;
239 for (
int i = 0; i < n; i++) {
241 double *pt = MIJ(r, i, i, ldr);
243 if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
251 for (
int k = 0; k < nsing; k++) {
252 int i = nsing - 1 - k;
253 wa1[i] /= *MIJ(r, i, i, ldr);
258 for (
unsigned int j = 0; j <= (
unsigned int)jm1; j++)
259 wa1[j] -= *MIJ(r, i, j, ldr) * temp;
264 for (
int j = 0; j < n; j++) {
276 for (
int i = 0; i < n; i++)
277 wa2[i] = diag[i] * x[i];
279 dxnorm = enorm(wa2, n);
281 fp = dxnorm - *delta;
283 if (fp > tol1 * (*delta)) {
292 for (
int i = 0; i < n; i++) {
294 wa1[i] = diag[l] * (wa2[l] / dxnorm);
297 for (
int i = 0; i < n; i++) {
303 for (
unsigned int j = 0; j <= (
unsigned int)im1; j++)
304 sum += (*MIJ(r, i, j, ldr) * wa1[j]);
306 wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
309 temp = enorm(wa1, n);
310 parl = ((fp / *delta) / temp) / temp;
317 for (
int i = 0; i < n; i++) {
320 for (
int j = 0; j <= i; j++)
321 sum += *MIJ(r, i, j, ldr) * qtb[j];
324 wa1[i] = sum / diag[l];
327 double gnorm = enorm(wa1, n);
328 double paru = gnorm / *delta;
331 if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
343 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon())
344 *par = gnorm / dxnorm;
358 if (std::fabs(*par) <= std::numeric_limits<double>::epsilon()) {
359 const double tol001 = 0.001;
365 for (
int i = 0; i < n; i++)
366 wa1[i] = temp * diag[i];
368 qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
370 for (
int i = 0; i < n; i++)
371 wa2[i] = diag[i] * x[i];
373 dxnorm = enorm(wa2, n);
375 fp = dxnorm - *delta;
386 if ((std::fabs(fp) <= tol1 * (*delta)) ||
387 ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp) && (temp < 0.0)) ||
402 for (
int i = 0; i < n; i++) {
404 wa1[i] = diag[l] * (wa2[l] / dxnorm);
407 for (
unsigned int i = 0; i < (
unsigned int)n; i++) {
408 wa1[i] = wa1[i] / sdiag[i];
410 unsigned int jp1 = i + 1;
411 if ((
unsigned int)n >= jp1) {
412 for (
unsigned int j = jp1; j < (
unsigned int)n; j++)
413 wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
417 temp = enorm(wa1, n);
418 double parc = ((fp / *delta) / temp) / temp;
460double pythag(
double a,
double b)
462 double pyth, p, r, t;
467 if (std::fabs(p) <= std::numeric_limits<double>::epsilon()) {
472 r = ((std::min)(std::fabs(a), std::fabs(b)) / p) * ((std::min)(std::fabs(a), std::fabs(b)) / p);
476 while (std::fabs(t - 4.0) < std::fabs(
vpMath::maximum(t, 4.0)) * std::numeric_limits<double>::epsilon()) {
478 double u = 1.0 + 2.0 * s;
480 r *= (s / u) * (s / u);
533int qrfac(
int m,
int n,
double *a,
int lda,
int *pivot,
int *ipvt,
int ,
double *rdiag,
double *acnorm,
536 const double tolerance = 0.05;
538 int i, j, ip1, k, kmax, minmn;
540 double sum, temp, tmp;
545 epsmch = std::numeric_limits<double>::epsilon();
551 for (i = 0; i < m; i++) {
552 acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
553 rdiag[i] = acnorm[i];
563 for (i = 0; i < minmn; i++) {
570 for (k = i; k < m; k++) {
571 if (rdiag[k] > rdiag[kmax])
576 for (j = 0; j < n; j++)
577 SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp);
579 rdiag[kmax] = rdiag[i];
582 SWAP(ipvt[i], ipvt[kmax], k);
590 double ajnorm = enorm(MIJ(a, i, i, lda), n - i);
593 if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon()) {
594 if (*MIJ(a, i, i, lda) < 0.0)
597 for (j = i; j < n; j++)
598 *MIJ(a, i, j, lda) /= ajnorm;
599 *MIJ(a, i, i, lda) += 1.0;
608 for (k = ip1; k < m; k++) {
610 for (j = i; j < n; j++)
611 sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
613 temp = sum / *MIJ(a, i, i, lda);
615 for (j = i; j < n; j++)
616 *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
619 if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon())) {
620 temp = *MIJ(a, k, i, lda) / rdiag[k];
623 if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch) {
624 rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - (
int)i));
695int qrsolv(
int n,
double *r,
int ldr,
int *ipvt,
double *diag,
double *qtb,
double *x,
double *sdiag,
double *wa)
699 double cosi, cotg, qtbpj, sinu, tg, temp;
706 for (i = 0; i < n; i++) {
707 for (j = i; j < n; j++)
708 *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
710 x[i] = *MIJ(r, i, i, ldr);
719 for (i = 0; i < n; i++) {
728 if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon()) {
729 for (k = i; k < n; k++)
743 for (k = i; k < n; k++) {
751 if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon()) {
752 if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k])) {
753 tg = sdiag[k] / *MIJ(r, k, k, ldr);
754 cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
757 cotg = *MIJ(r, k, k, ldr) / sdiag[k];
758 sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
767 *MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k];
768 temp = cosi * wa[k] + sinu * qtbpj;
769 qtbpj = -sinu * wa[k] + cosi * qtbpj;
780 for (j = kp1; j < n; j++) {
781 temp = cosi * *MIJ(r, k, j, ldr) + sinu * sdiag[j];
782 sdiag[j] = -sinu * *MIJ(r, k, j, ldr) + cosi * sdiag[j];
783 *MIJ(r, k, j, ldr) = temp;
794 sdiag[i] = *MIJ(r, i, i, ldr);
795 *MIJ(r, i, i, ldr) = x[i];
804 for (i = 0; i < n; i++) {
806 if ((std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
814 for (k = 0; k < nsing; k++) {
820 for (j = jp1; j < nsing; j++)
821 sum += *MIJ(r, i, j, ldr) * wa[j];
823 wa[i] = (wa[i] - sum) / sdiag[i];
830 for (j = 0; j < n; j++) {
943int lmder(
void (*ptr_fcn)(
int m,
int n,
double *xc,
double *fvecc,
double *jac,
int ldfjac,
int iflag),
int m,
int n,
944 double *x,
double *fvec,
double *fjac,
int ldfjac,
double ftol,
double xtol,
double gtol,
unsigned int maxfev,
945 double *diag,
int mode,
const double factor,
int nprint,
int *info,
unsigned int *nfev,
int *njev,
int *ipvt,
946 double *qtf,
double *wa1,
double *wa2,
double *wa3,
double *wa4)
948 const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
953 double actred, delta, dirder, epsmch, fnorm, fnorm1;
954 double ratio = std::numeric_limits<double>::epsilon();
955 double par, pnorm, prered;
956 double sum, temp, temp1, temp2, xnorm = 0.0;
959 epsmch = std::numeric_limits<double>::epsilon();
984 if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0) || (maxfev == 0) ||
995 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1001 for (j = 0; j < n; j++) {
1002 if (diag[j] <= 0.0) {
1012 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1025 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1039 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1044 fnorm = enorm(fvec, m);
1057 while (count < (
int)maxfev) {
1065 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1079 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1089 if ((iter - 1) % nprint == 0)
1090 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1102 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1111 qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3);
1120 for (j = 0; j < n; j++) {
1123 if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
1134 for (j = 0; j < n; j++)
1135 wa3[j] = diag[j] * x[j];
1137 xnorm = enorm(wa3, n);
1138 delta = factor * xnorm;
1141 if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
1149 for (i = 0; i < m; i++)
1152 for (i = 0; i < n; i++) {
1153 double *pt = MIJ(fjac, i, i, ldfjac);
1155 if (std::fabs(*pt) > std::numeric_limits<double>::epsilon()) {
1158 for (j = i; j < m; j++)
1159 sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
1161 temp = -sum / *MIJ(fjac, i, i, ldfjac);
1163 for (j = i; j < m; j++)
1164 wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
1167 *MIJ(fjac, i, i, ldfjac) = wa1[i];
1178 if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon()) {
1179 for (i = 0; i < n; i++) {
1182 if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon()) {
1184 for (j = 0; j <= i; j++)
1185 sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
1209 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1219 for (j = 0; j < n; j++)
1227 while (ratio < tol0001) {
1232 lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4);
1238 for (j = 0; j < n; j++) {
1240 wa2[j] = x[j] + wa1[j];
1241 wa3[j] = diag[j] * wa1[j];
1244 pnorm = enorm(wa3, n);
1259 (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
1271 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1276 fnorm1 = enorm(wa4, m);
1284 if ((tol1 * fnorm1) < fnorm)
1285 actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
1292 for (i = 0; i < n; i++) {
1296 for (j = 0; j <= i; j++)
1297 wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
1300 temp1 = enorm(wa3, n) / fnorm;
1301 temp2 = (sqrt(par) * pnorm) / fnorm;
1302 prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
1303 dirder = -((temp1 * temp1) + (temp2 * temp2));
1312 if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
1313 ratio = actred / prered;
1319 if (ratio > tol25) {
1321 if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75)) {
1322 delta = pnorm / tol5;
1330 temp = tol5 * dirder / (dirder + tol5 * actred);
1332 if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
1342 if (ratio >= tol0001) {
1348 for (j = 0; j < n; j++) {
1350 wa2[j] = diag[j] * x[j];
1353 for (i = 0; i < m; i++)
1356 xnorm = enorm(wa2, n);
1365 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
1368 if (delta <= xtol * xnorm)
1371 if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2)
1384 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1393 if (*nfev >= maxfev)
1396 if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0))
1399 if (delta <= epsmch * xnorm)
1402 if (gnorm <= epsmch)
1415 (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
1505int lmder1(
void (*ptr_fcn)(
int m,
int n,
double *xc,
double *fvecc,
double *jac,
int ldfjac,
int iflag),
int m,
int n,
1506 double *x,
double *fvec,
double *fjac,
int ldfjac,
double tol,
int *info,
int *ipvt,
int lwa,
double *wa)
1508 const double factor = 100.0;
1509 unsigned int maxfev, nfev;
1510 int mode, njev, nprint;
1511 double ftol, gtol, xtol;
1517 if ( (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < (5 * n + m))) {
1518 printf(
"%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < (5 * n + m)));
1524 maxfev = (
unsigned int)(100 * (n + 1));
1531 lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev,
1532 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)