minpack.cpp

有关摄像头定标的C++代码

#include "stdafx.h"
#include "math.h"
#include "f2c.h" 
#include "minpack.h"
#include "f:\practice\yang2\cal_main.h"

static long c__1 = 1;
static long c__2 = 1;
static long c__3 = 2;
static long c__4 = 1;


int qrsolv_(long *n, double *r, long *ldr, long *ipvt, double *diag, double *qtb, 
                    double *x, double *sdiag, double *wa)
{
    /* Initialized data */
    static double p5 = .5;
    static double p25 = .25;
    static double zero = 0.;

    /* System generated locals */
    long r_dim1, r_offset, i__1, i__2, i__3;
    double d__1, d__2;

    /* Builtin functions */

    /* Local variables */
    static double temp;
    static long i, j, k, l;
    static double cotan;
    static long nsing;
    static double qtbpj;
    static long jp1, kp1;
    static double tan_, cos_, sin_, sum;

    /* Parameter adjustments */
    --wa;
    --sdiag;
    --x;
    --qtb;
    --diag;
    --ipvt;
    r_dim1 = *ldr;
    r_offset = r_dim1 + 1;
    r -= r_offset;

    /* Function Body */

    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	i__2 = *n;
	for (i = j; i <= i__2; ++i) {
	    r[i + j * r_dim1] = r[j + i * r_dim1];
/* L10: */
	}
	x[j] = r[j + j * r_dim1];
	wa[j] = qtb[j];
/* L20: */
    }

/*     eliminate the diagonal matrix d using a givens rotation. */

    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {

/*        prepare the row of d to be eliminated, locating the */
/*        diagonal element using p from the qr factorization. */

	l = ipvt[j];
	if (diag[l] == zero) {
	    goto L90;
	}
	i__2 = *n;
	for (k = j; k <= i__2; ++k) {
	    sdiag[k] = zero;
/* L30: */
	}
	sdiag[j] = diag[l];

/*        the transformations to eliminate the row of d */
/*        modify only a single element of (q transpose)*b */
/*        beyond the first n, which is initially zero. */

	qtbpj = zero;
	i__2 = *n;
	for (k = j; k <= i__2; ++k) {

/*           determine a givens rotation which eliminates the */
/*           appropriate element in the current row of d. */

	    if (sdiag[k] == zero) {
		goto L70;
	    }
	    if ((d__1 = r[k + k * r_dim1], abs(d__1)) >= (d__2 = sdiag[k], 
		    abs(d__2))) {
		goto L40;
	    }
	    cotan = r[k + k * r_dim1] / sdiag[k];
/* Computing 2nd power */
	    d__1 = cotan;
	    sin_ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
	    cos_ = sin_ * cotan;
	    goto L50;
L40:
	    tan_ = sdiag[k] / r[k + k * r_dim1];
/* Computing 2nd power */
	    d__1 = tan_;
	    cos_ = p5 / sqrt(p25 + p25 * (d__1 * d__1));
	    sin_ = cos_ * tan_;
L50:

/*           compute the modified diagonal element of r and */
/*           the modified element of ((q transpose)*b,0). */

	    r[k + k * r_dim1] = cos_ * r[k + k * r_dim1] + sin_ * sdiag[k];
	    temp = cos_ * wa[k] + sin_ * qtbpj;
	    qtbpj = -sin_ * wa[k] + cos_ * qtbpj;
	    wa[k] = temp;

/*           accumulate the tranformation in the row of s. */

	    kp1 = k + 1;
	    if (*n < kp1) {
		goto L70;
	    }
	    i__3 = *n;
	    for (i = kp1; i <= i__3; ++i) {
		temp = cos_ * r[i + k * r_dim1] + sin_ * sdiag[i];
		sdiag[i] = -sin_ * r[i + k * r_dim1] + cos_ * sdiag[i];
		r[i + k * r_dim1] = temp;
/* L60: */
	    }
L70:
/* L80: */
	    ;
	}
L90:

/*        store the diagonal element of s and restore */
/*        the corresponding diagonal element of r. */

	sdiag[j] = r[j + j * r_dim1];
	r[j + j * r_dim1] = x[j];
/* L100: */
    }

/*     solve the triangular system for z. if the system is */
/*     singular, then obtain a least squares solution. */

    nsing = *n;
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	if (sdiag[j] == zero && nsing == *n) {
	    nsing = j - 1;
	}
	if (nsing < *n) {
	    wa[j] = zero;
	}
/* L110: */
    }
    if (nsing < 1) {
	goto L150;
    }
    i__1 = nsing;
    for (k = 1; k <= i__1; ++k) {
	j = nsing - k + 1;
	sum = zero;
	jp1 = j + 1;
	if (nsing < jp1) {
	    goto L130;
	}
	i__2 = nsing;
	for (i = jp1; i <= i__2; ++i) {
	    sum += r[i + j * r_dim1] * wa[i];
/* L120: */
	}
L130:
	wa[j] = (wa[j] - sum) / sdiag[j];
/* L140: */
    }
L150:

/*     permute the components of z back to components of x. */

    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	l = ipvt[j];
	x[l] = wa[j];
/* L160: */
    }
    return 0;
} 


double dpmpar_(long *i)
{
	static struct {
	long e_1[6];
	double fill_2[1];
	double e_3;
	} equiv_2 = { 1018167296, 0, 1048576, 0, 2146435071, -1, {0}, 0. };


    /* System generated locals */
    double ret_val;

    /* Local variables */
#define dmach ((double *)&equiv_2)
#define minmag ((long *)&equiv_2 + 2)
#define maxmag ((long *)&equiv_2 + 4)
#define mcheps ((long *)&equiv_2)

    ret_val = dmach[*i - 1];
    return ret_val;
} 
#undef mcheps
#undef maxmag
#undef minmag
#undef dmach

double enorm_(long *n, double *x)
{
    /* Initialized data */

    static double one = 1.;
    static double zero = 0.;
    static double rdwarf = 3.834e-20;
    static double rgiant = 1.304e19;

    /* System generated locals */
    long i__1;
    double ret_val, d__1;

    /* Builtin functions */

    /* Local variables */
    static double xabs, x1max, x3max;
    static long i;
    static double s1, s2, s3, agiant, floatn;

    /* Parameter adjustments */
    --x;

    /* Function Body */
    s1 = zero;
    s2 = zero;
    s3 = zero;
    x1max = zero;
    x3max = zero;
    floatn = (double) (*n);
    agiant = rgiant / floatn;
    i__1 = *n;
    for (i = 1; i <= i__1; ++i) {
	xabs = (d__1 = x[i], abs(d__1));
	if (xabs > rdwarf && xabs < agiant) {
	    goto L70;
	}
	if (xabs <= rdwarf) {
	    goto L30;
	}

/*              sum for large components. */

	if (xabs <= x1max) {
	    goto L10;
	}
/* Computing 2nd power */
	d__1 = x1max / xabs;
	s1 = one + s1 * (d__1 * d__1);
	x1max = xabs;
	goto L20;
L10:
/* Computing 2nd power */
	d__1 = xabs / x1max;
	s1 += d__1 * d__1;
L20:
	goto L60;
L30:

/*              sum for small components. */

	if (xabs <= x3max) {
	    goto L40;
	}
/* Computing 2nd power */
	d__1 = x3max / xabs;
	s3 = one + s3 * (d__1 * d__1);
	x3max = xabs;
	goto L50;
L40:
	if (xabs != zero) {
/* Computing 2nd power */
	    d__1 = xabs / x3max;
	    s3 += d__1 * d__1;
	}
L50:
L60:
	goto L80;
L70:

/*           sum for intermediate components. */

/* Computing 2nd power */
	d__1 = xabs;
	s2 += d__1 * d__1;
L80:
/* L90: */
	;
    }

/*     calculation of norm. */

    if (s1 == zero) {
	goto L100;
    }
    ret_val = x1max * sqrt(s1 + s2 / x1max / x1max);
    goto L130;
L100:
    if (s2 == zero) {
	goto L110;
    }
    if (s2 >= x3max) {
	d__1 = x3max * s3;
	ret_val = sqrt(s2 * (one + x3max / s2 * d__1));
    }
    if (s2 < x3max) {
	ret_val = sqrt(x3max * (s2 / x3max + x3max * s3));
    }
    goto L120;
L110:
    ret_val = x3max * sqrt(s3);
L120:
L130:
    return ret_val;
} 

int fdjac2_(void(*fcn)(long*,long*,double*,double*), long *m, long *n, double *x, double *fvec, double *fjac, long *ldfjac, 
			long *iflag, double *epsfcn, double *wa)
{
    static double zero = 0.;

    long fjac_dim1, fjac_offset, i__1, i__2;

    /* Builtin functions */

    /* Local variables */
    static double temp, h;
    static long i, j;
    static double epsmch;
    static double eps;

    /* Parameter adjustments */
    --wa;
    fjac_dim1 = *ldfjac;
    fjac_offset = fjac_dim1 + 1;
    fjac -= fjac_offset;
    --fvec;
    --x;

    /* Function Body */

/*     epsmch is the machine precision. */

    epsmch = dpmpar_(&c__1);

    eps = sqrt((max(*epsfcn,epsmch)));
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	temp = x[j];
	h = eps * abs(temp);
	if (h == zero) {
	    h = eps;
	}
	x[j] = temp + h;

	//(*fcn)(m, n, &x[1], &wa[1], iflag);
	(*fcn)(m, n, &x[1], &wa[1]);

	if (*iflag < 0) {
	    goto L30;
	}
	x[j] = temp;
	i__2 = *m;
	for (i = 1; i <= i__2; ++i) {
	    fjac[i + j * fjac_dim1] = (wa[i] - fvec[i]) / h;
/* L10: */
	}
/* L20: */
    }
L30:
    return 0;
} 

int lmdif_(void(*fcn)(long*,long*,double*,double*), long *m, long *n, double *x, double *fvec, double *ftol, double *xtol, double *gtol,
		   long *maxfev,double *epsfcn, double *diag, long *mode, double *factor, long *nprint, long *info, 
		   long *nfev, double *fjac, long *ldfjac, long *ipvt,double *qtf, double *wa1, double *wa2,
		   double *wa3, double *wa4)
{
    /* Initialized data */
    static double one = 1.;
    static double p1 = .1;
    static double p5 = .5;
    static double p25 = .25;
    static double p75 = .75;
    static double p0001 = 1e-4;
    static double zero = 0.;

    /* System generated locals */
    long fjac_dim1, fjac_offset, i__1, i__2;
    double d__1, d__2, d__3;

    /* Builtin functions */

    /* Local variables */
    static long iter;
    static double temp, temp1, temp2;
    static long i, j, l, iflag;
    static double delta;
    static double ratio;
    static double fnorm, gnorm;
    static double pnorm, xnorm, fnorm1, actred, dirder, epsmch, prered;
    static double par, sum;

    --wa4;
    --wa3;
    --wa2;
    --wa1;
    --qtf;
    --ipvt;
    fjac_dim1 = *ldfjac;
    fjac_offset = fjac_dim1 + 1;
    fjac -= fjac_offset;
    --diag;
    --fvec;
    --x;

    /* Function Body */

/*     epsmch is the machine precision. */

    epsmch = dpmpar_(&c__2);

    *info = 0;
    iflag = 0;
    *nfev = 0;

/*     check the input parameters for errors. */

    if (*n <= 0 || *m < *n || *ldfjac < *m || *ftol < zero || *xtol < zero || 
	    *gtol < zero || *maxfev <= 0 || *factor <= zero) {
	goto L300;
    }
    if (*mode != 2) {
	goto L20;
    }
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	if (diag[j] <= zero) {
	    goto L300;
	}
/* L10: */
    }
L20:

/*     evaluate the function at the starting point */
/*     and calculate its norm. */

    iflag = 1;

    //(*fcn)(m, n, &x[1], &fvec[1], &iflag);
    (*fcn)(m, n, &x[1], &fvec[1]);

    *nfev = 1;
    if (iflag < 0) {
	goto L300;
    }
    fnorm = enorm_(m, &fvec[1]);

/*     initialize levenberg-marquardt parameter and iteration counter. */

    par = zero;
    iter = 1;

/*     beginning of the outer loop. */

L30:

/*        calculate the jacobian matrix. */

    iflag = 2;
    fdjac2_(fcn, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, 
	    epsfcn, &wa4[1]);
    *nfev += *n;
    if (iflag < 0) {
	goto L300;
    }

/*        if requested, call fcn to enable printing of iterates. */

    if (*nprint <= 0) {
	goto L40;
    }
    iflag = 0;
    if ((iter - 1) % *nprint == 0) {
	//(*fcn)(m, n, &x[1], &fvec[1], &iflag);
	(*fcn)(m, n, &x[1], &fvec[1]);
    }
    if (iflag < 0) {
	goto L300;
    }
L40:

/*        compute the qr factorization of the jacobian. */

    qrfac_(m, n, &fjac[fjac_offset], ldfjac, &c__2, &ipvt[1], n, &wa1[1], &
	    wa2[1], &wa3[1]);

/*        on the first iteration and if mode is 1, scale according */
/*        to the norms of the columns of the initial jacobian. */

    if (iter != 1) {
	goto L80;
    }
    if (*mode == 2) {
	goto L60;
    }
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	diag[j] = wa2[j];
	if (wa2[j] == zero) {
	    diag[j] = one;
	}
/* L50: */
    }
L60:

/*        on the first iteration, calculate the norm of the scaled x */
/*        and initialize the step bound delta. */

    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	wa3[j] = diag[j] * x[j];
/* L70: */
    }
    xnorm = enorm_(n, &wa3[1]);
    delta = *factor * xnorm;
    if (delta == zero) {
	delta = *factor;
    }
L80:

/*        form (q transpose)*fvec and store the first n components in */
/*        qtf. */

    i__1 = *m;
    for (i = 1; i <= i__1; ++i) {
	wa4[i] = fvec[i];
/* L90: */
    }
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	if (fjac[j + j * fjac_dim1] == zero) {
	    goto L120;
	}
	sum = zero;
	i__2 = *m;
	for (i = j; i <= i__2; ++i) {
	    sum += fjac[i + j * fjac_dim1] * wa4[i];
/* L100: */
	}
	temp = -sum / fjac[j + j * fjac_dim1];
	i__2 = *m;
	for (i = j; i <= i__2; ++i) {
	    wa4[i] += fjac[i + j * fjac_dim1] * temp;
/* L110: */
	}
L120:
	fjac[j + j * fjac_dim1] = wa1[j];
	qtf[j] = wa4[j];
/* L130: */
    }

/*        compute the norm of the scaled gradient. */

    gnorm = zero;
    if (fnorm == zero) {
	goto L170;
    }
    i__1 = *n;
    for (j = 1; j <= i__1; ++j) {
	l = ipvt[j];
	if (wa2[l] == zero) {
	    goto L150;
	}
	sum = zero;
	i__2 = j;
	for (i = 1; i <= i__2; ++i) {
	    sum += fjac[i + j * fjac_dim1] * (qtf[i] / fnorm);
/* L140: */
	}
/* Computing MAX */
	d__2 = gnorm, d__3 = (d__1 = sum / wa2[l], abs(d__1));
	gnorm = max(d__2,d__3);
L150:
/* L160: */
	;
    }
L170:

/*        test for convergence of the gradient norm. */

    if (gnorm <= *gtol) {
	*info = 4;
    }
    if (*info != 0) {
	goto L300;
    }

/*        rescale if necessary. */

    if (*mode == 2) {