home *** CD-ROM | disk | FTP | other *** search
/ vis-ftp.cs.umass.edu / vis-ftp.cs.umass.edu.tar / vis-ftp.cs.umass.edu / pub / Software / ASCENDER / ascender.tar.Z / ascender.tar / Triangulate / marqu.c < prev    next >
C/C++ Source or Header  |  1995-04-13  |  14KB  |  536 lines
  1. /* @(#)marqu.c    1.17 12/20/94 */
  2. /* Routines for carrying out the Marquardt algorithm */
  3.  
  4. #include <math.h>
  5. #include "rh_util.h"
  6. #include "marquardt.h"
  7. #include "array_utils.s"
  8. #include "error_message.s"
  9. #include "newgaussj.s"
  10. #include "newgaussj2.s"
  11. #include "svalue.s"
  12. /* #include "utilities_c.h" */
  13.  
  14. /* Some local variables */
  15. static double **weight;
  16. static double *goal;
  17. static double *mparam;
  18. static double **covar;
  19. static int nrows, ncols;
  20. static int Num_Loops = 100;
  21. static double Min_Incr = 0.05;
  22. static BOOLEAN diag;
  23.  
  24. /*  Some forward declarations */
  25. FUNCTION_DECL (static void (*gen_jacobian), (
  26.    double *mparam,
  27.    double **dyda,
  28.    int nrows,
  29.    int ncols
  30.    ));
  31.  
  32. FUNCTION_DECL (static void (*function), (
  33.    double *mparams,
  34.    double *newvalue,
  35.    int nrows,
  36.    int ncols));
  37.  
  38. FUNCTION_DECL (static void (*new_position_action), (
  39.    double *mparam,
  40.    int ncols));
  41.  
  42. FUNCTION_DECL (static void mrqcof, (
  43.    double **weight, 
  44.    double **alpha, 
  45.    double *beta, 
  46.    double **dyda, 
  47.    double *dy,
  48.    int nrows, 
  49.    int ncols,
  50.    int diag));
  51.  
  52. FUNCTION_DECL (static double calculate_error, (double *del));
  53.  
  54. /* Routines for handling an interrupt to tell us to stop after next iteration */
  55. #include <signal.h>
  56. static BOOLEAN interrupt_pending = FALSE;
  57.  
  58. FUNCTION_DEF ( static void keybd_interrupt , (int errno))
  59.    {
  60.    if (errno == SIGTSTP) interrupt_pending = TRUE;
  61.    }
  62.  
  63. FUNCTION_DEF ( static void calculate_new_position, (
  64.    double *dp,
  65.    double *mparam))    /* Parameter values to be updated */
  66.    {
  67.    /* Calculate the new positions from the new values of the parameters */
  68.    int i;
  69.  
  70.    /* Update the solution */
  71.    for (i=1; i<=ncols; i++)
  72.       mparam[i] += dp[i];
  73.  
  74.    }
  75.   
  76. FUNCTION_DEF (static void compute_error_vector, (
  77.    double *mparams,    /* The parameters used for the computation */
  78.    double *dy,        /* The error vector */
  79.    int nrows,         /* Number of elements in mparams */
  80.    int ncols))        /* Number of elements in dy */
  81.    {
  82.    /* Determines what the error would be if atry were the new position */
  83.    double *newvalue = VECTOR (1, nrows, double);
  84.    int i;
  85.  
  86.    /* Call the function to compute the function */
  87.    (*function) (mparams, newvalue, nrows, ncols);
  88.  
  89.    /* Now compute the error vector */
  90.    for (i=1; i<=nrows; i++)
  91.       dy[i] = goal[i] - newvalue[i];
  92.    
  93.    /* Free the temporary */
  94.    FREE (newvalue, 1);
  95.    }
  96.  
  97. FUNCTION_DEF (static double calculate_error, (double *del))
  98.    {
  99.    /* Computes the weighted value of an error vector del */
  100.    double error = 0.0;
  101.    int i;
  102.  
  103.    /* Now compute the weighted error */
  104.    if (diag)
  105.       for (i=1; i<=nrows; i++)
  106.      error += del[i] * del[i] * weight[1][i];
  107.    else
  108.       for (i=1; i<=nrows; i++)
  109.      {
  110.      int j;
  111.      /* Add twice the off-diagonal error components */
  112.      for (j=1; j<i; j++)
  113.         error += 2.0 * del[i] * del[j] * weight[i][j];
  114.  
  115.      /* Add once the diagonal contribution */
  116.      error += del[i] * del[i] * weight[i][i];
  117.      }
  118.  
  119.    /* The error computation is now complete */
  120.    return (error);
  121.    }
  122.      
  123. FUNCTION_DEF (static int compute_covariance, (
  124.    double **dyda,         /* Jacobian */
  125.    double *dy,            /* Error vector */
  126.    double **weight,        /* Weight matrix or vector */
  127.    BOOLEAN diag,        /* Tells whether weight matrix is diagonal */
  128.    int nrows, int ncols,    /* Dimensions of the Jacobian matrix */
  129.    double **covar        /* Returned covariance matrix.  Must already be
  130.                  * allocated */
  131.    ))
  132.    {
  133.    /* This is the Marquardt Minimization algorithm.  Please read the comment
  134.     * in the book Numerical recipes, page 545.
  135.     */
  136.    static double oldchisq;
  137.    double *beta;
  138.    int rval;
  139.  
  140.    /* Return if covar is not allocated */
  141.    if (covar == (double **)NULL) return 0;
  142.    
  143.    /* Allocate an array */
  144.    beta = VECTOR (1, ncols, double);
  145.  
  146.    /* Calculate the matrices */
  147.    mrqcof (weight, covar, beta, dyda, dy, nrows, ncols, diag);
  148.  
  149.    /* Use gaussj to invert the covar matrix */
  150.    rval = gaussj_check(covar, ncols, beta);
  151.  
  152.    /* Free the temporary */
  153.    FREE (beta, 1);
  154.  
  155.    /* Return the return value */
  156.    return (rval);
  157.    }
  158.  
  159. FUNCTION_DEF (static void mrqcof, (
  160.    double **weight, 
  161.    double **alpha, 
  162.    double *beta, 
  163.    double **dyda, 
  164.    double *dy,
  165.    int nrows, 
  166.    int ncols,
  167.    int diag))
  168.    {
  169.    /* Routine used by mrqmin to evaluate the linearized fitting matrix 
  170.     * alpha[1..ncols][1..ncols] and vector beta[1..ncols] 
  171.     * as in Cookbook, (14.4.8). */
  172.  
  173.    /* Initialize (symmetric) alpha, beta */
  174.       {
  175.       int j, k;
  176.       for (j=1; j<=ncols; j++)
  177.          for (k=1; k<=j; k++) 
  178.         alpha[j][k] = beta[j] = 0.0;
  179.       }
  180.  
  181.       /* Compute the alpha and beta matrices */
  182.  
  183.    if (diag)
  184.       {
  185.       /* Calculate J^T*J and J^T*dy */
  186.       register int i;
  187.       for (i=1; i<=nrows; i++)
  188.          {
  189.          /* summation loop over all data */
  190.        register int j;
  191.          for (j=1; j<=ncols; j++)
  192.         {
  193.          register int k;
  194.         double wt = dyda[i][j]*weight[1][i];
  195.         for (k=1; k<=j; k++)
  196.            alpha[j][k] += wt*dyda[i][k];
  197.         beta[j] += dy[i]*wt;
  198.         }
  199.          }
  200.       }
  201.    else
  202.       {
  203.       /* Compute  alpha = J^T * C * J and beta = J^T * C * dy */
  204.       register int i;
  205.       for (i=1; i<=ncols; i++)
  206.      {
  207.      int k;
  208.      for (k=1; k<=nrows; k++)
  209.         {
  210.         int j;
  211.         double JTC_i_k = 0.0;
  212.  
  213.         /* Compute the i,k-th entry of J^T * C */
  214.         for (j=1; j<=nrows; j++)
  215.            JTC_i_k += dyda[j][i] * weight[j][k];
  216.  
  217.         /* Now multiply by J */
  218.         for (j=1; j<=i; j++)
  219.            alpha[i][j] += JTC_i_k * dyda[k][j];
  220.  
  221.         /* Compute J^T*C*dy */
  222.         beta[i] += JTC_i_k * dy[k];
  223.         } 
  224.      }
  225.       }
  226.  
  227.    /* Fill in the symmetric side */
  228.       {
  229.       register int j, k;
  230.       for (j=2; j<=ncols; j++)
  231.          for (k=1; k<=j-1; k++) alpha[k][j] = alpha[j][k];
  232.       }
  233.    }
  234.  
  235. #define FAC   1.0e-6
  236. #define DELTA 1.0e-6
  237.  
  238. FUNCTION_DEF (static void compute_jacobian, (
  239.    double *mparam,
  240.    double **dyda,
  241.    int nrows, int ncols))
  242.    {
  243.    /* Computes the jacobian by approximation */
  244.    int i, j;
  245.    double *oldval = VECTOR (1, nrows, double);
  246.    double *newval = VECTOR (1, nrows, double);
  247.  
  248.    /* Evaluate the function at the current point */
  249.    (*function) (mparam, oldval, nrows, ncols);
  250.  
  251.    /* Now change the parameters one after another */
  252.    for (j=1; j<=ncols; j++)
  253.       {
  254.       /* Remember the value of the j-th parameter */
  255.       double oldparam = mparam[j];
  256.  
  257.       /* Increment the j-th parameter */
  258.       double delta = FAC * mparam[j];
  259.       if (fabs (delta) < DELTA) delta = DELTA;
  260.       mparam[j] += delta;
  261.  
  262.       /* Now recompute the function */
  263.       (*function) (mparam, newval, nrows, ncols);
  264.  
  265.       /* Now fill out the jacobian values */
  266.       for (i=1; i<=nrows; i++)
  267.      dyda[i][j] = (newval[i] - oldval[i]) / delta;
  268.       
  269.       /* Restore the value of mparam[j] */
  270.       mparam[j] = oldparam;
  271.       }
  272.  
  273.    /* Free the temporaries */
  274.    FREE (oldval, 1);
  275.    FREE (newval, 1);
  276.    }
  277.    
  278. FUNCTION_DEF (static double marquardt_go, ())
  279.    {
  280.    /* Declare variables */
  281.    int icount;
  282.    double alamda = 1.0;
  283.    double chisq;
  284.    BOOLEAN improvement = TRUE;
  285.  
  286.    /* Allocate a bunch of storage */
  287.    double *da         = VECTOR (1, ncols, double);   
  288.    double **dyda      = MATRIX (1, nrows, 0, ncols, double);
  289.    double *dy         = VECTOR (1, nrows, double);
  290.    double *newparams     = VECTOR (1, ncols, double);
  291.    double *newdy        = VECTOR (1, nrows, double);
  292.    double **aprime     = MATRIX (1, ncols, 1, ncols, double);
  293.    double *beta        = VECTOR (1, ncols, double);
  294.    double **alpha     = MATRIX (1, ncols, 1, ncols, double);
  295.  
  296.    /* Now iterate */
  297.    for (icount=0; icount<Num_Loops && improvement; icount++)
  298.       {
  299.       int j;
  300.       double increment;
  301.    
  302.       /* Compute the error vector */
  303.       compute_error_vector (mparam, dy, nrows, ncols);
  304.  
  305.       /* Compute an initial value of the error */
  306.       chisq = calculate_error (dy);
  307.  
  308.       /* Output some debugging info */
  309.       if (icount == 0)
  310.          informative_message ("\tInitial error : chisq = %g, rms = %g", 
  311.         chisq, sqrt(chisq/nrows));
  312.  
  313.       /* Compute the Jacobian */
  314.       if (gen_jacobian)
  315.          (*gen_jacobian) (mparam, dyda, nrows, ncols);
  316.       else
  317.          compute_jacobian (mparam, dyda, nrows, ncols);
  318.  
  319. #ifdef WRITE_JACOBIAN
  320.       fprintf (stderr, "\nJacobian (%d x %d) : \n", nrows, ncols);
  321.       fprint_matrix ("%11.3f ", stderr, dyda, 1, nrows, 1, ncols);
  322. #endif
  323.  
  324.       /* Calculate the matrices */
  325.       mrqcof (weight, alpha, beta, dyda, dy, nrows, ncols, diag);
  326.  
  327.       /* Now estimate new position with different alambda until improvement */
  328.       do {
  329.          double newchisq;
  330.          int i;
  331.      /* Copy the values of alpha to aprime, the augmented alpha */
  332.      for (i=1; i<=ncols; i++)
  333.         {
  334.         /* Copy beta */
  335.               da[i] = beta[i];
  336.  
  337.         /* Copy alpha */
  338.         for (j=1; j<=ncols; j++)
  339.            aprime[i][j] = alpha[i][j];
  340.         }
  341.           
  342.          /* Augment diagonal elements of aprime */
  343.          for (j=1; j<=ncols; j++)
  344.             aprime[j][j] *= 1.0 + (alamda);
  345.  
  346.          /* Matrix solution.  Solution overwrites beta, now called da */
  347.          if (! (lin_solve(aprime, da, da, ncols)
  348.                 || solve_simple (aprime, da, da, ncols, ncols)))
  349.             {
  350.             error_message ("Can not solve normal equations -- exiting");
  351.             bail_out (1);
  352.             }
  353.  
  354.          /* Compute the length of the increment */
  355.          increment = 0.0;
  356.          for (j=1; j<=ncols; j++)
  357.         increment += da[j] * da[j];
  358.  
  359.          /* Calculate new error at this position */
  360.             {
  361.         int i;
  362.  
  363.         /* Add the delta to get new provisional parameter set */
  364.             for (i=1; i<=ncols; i++)
  365.                newparams[i] = mparam[i] + da[i];
  366.  
  367.         /* Compute the error vector at this value */
  368.             compute_error_vector (newparams, newdy, nrows, ncols);
  369.  
  370.             /* Get the value of chisq resulting from this error vector */
  371.             newchisq = calculate_error(newdy);
  372.         }
  373.  
  374.          /* Accept or reject it */
  375.          if (newchisq < chisq)
  376.             {
  377.             /* Success, accept this new solution */
  378.             alamda *= 0.1;
  379.             chisq = newchisq;
  380.        
  381.         /* Accept the new parameter values */
  382.         for (i=1; i<=ncols; i++) mparam[i] = newparams[i];
  383.    
  384.             /* Call any user function */
  385.             if (new_position_action)
  386.                (*new_position_action) (mparam, ncols);
  387.  
  388.         /* Signal improvement */
  389.         improvement = TRUE;
  390.             }
  391.          else
  392.             {
  393.             /* Failure, increas alamda and return. */
  394.             alamda *= 10.0;
  395.         improvement = FALSE;
  396.             }
  397.          } while ( ! improvement && increment >= Min_Incr && (alamda < 1e10));
  398.         
  399.       /* Output some debugging info */
  400.       informative_message ("\talamda = %5.0e, chisq = %g, rms = %g", 
  401.      alamda, chisq, sqrt(chisq/nrows));
  402.  
  403.       /* Test for an interrupt */
  404.       if (interrupt_pending) break;
  405.  
  406.       /* Test to see if the increment has been too small */
  407.       if (increment < Min_Incr) break;
  408.       }
  409.  
  410.    /* Compute the covariance matrix */
  411.    if (covar)
  412.       if (! compute_covariance(dyda, dy, weight, diag, nrows, ncols, covar))
  413.          error_message ("Can not compute covariance matrix");
  414.  
  415.    /* Free the storage */
  416.    FREE(da,1);
  417.    FREE(dyda,1);
  418.    FREE(dy,1);
  419.    FREE(newparams,1);
  420.    FREE(newdy,1);
  421.    FREE(aprime,1);
  422.    FREE(beta,1);    
  423.    FREE(alpha,1);
  424.  
  425.    /* Return the value of the error */
  426.    return chisq;
  427.    }
  428.  
  429. FUNCTION_DEF (double marquardt, (Marquardt_info *mar))
  430.    {
  431.    /* Do marquardt iteration */
  432.  
  433.    double return_val;            /* The error of fit */
  434.    BOOLEAN free_weights = FALSE;    /* Free if we allocated here */
  435.  
  436.    /* Pick the local variables off the marquardt descriptor structure */
  437.    if (mar->Num_Loops > 0) Num_Loops = mar->Num_Loops;
  438.    if (mar->Min_Incr >= 0.0) Min_Incr = mar->Min_Incr;
  439.    nrows = mar->nrows;
  440.    ncols = mar->ncols;
  441.    weight = mar->weight;
  442.    goal = mar->goal;
  443.    mparam = mar->param;
  444.    diag = mar->diag;
  445.    covar = mar->covar;
  446.    gen_jacobian = mar->gen_jacobian;
  447.    function = mar->function;
  448.    new_position_action = mar->new_position_action;
  449.  
  450.    /* Set up the interrupt_pending value */
  451.    interrupt_pending = FALSE;
  452.    signal (SIGTSTP, /*(SIG_PF)*/ keybd_interrupt);
  453.  
  454.    /* If the weight matrix is not known, then make it the identity matrix */
  455.    if (! weight)
  456.       {
  457.       int i;
  458.       weight = MATRIX (1, 1, 1, nrows, double);
  459.       free_weights = TRUE;
  460.       for (i=1; i<=nrows; i++) weight[1][i] = 1.0;
  461.       diag = TRUE;
  462.       }
  463.  
  464.    /* Now that house-keeping is done, start the routine */
  465.    return_val = marquardt_go();
  466.  
  467.    /* Free the weights */
  468.    if (free_weights) FREE (weight, 1);
  469.  
  470.    /* Return success of failure */
  471.    return return_val;
  472.    }
  473.  
  474. FUNCTION_DEF (void marquardt_debug, (
  475.    double **alpha, 
  476.    double *beta))
  477.    {
  478.    /* Outputs debug info to test the new algorithm */
  479.    int i, j;
  480.    int d1 = 12;
  481.    int d2 = 24;
  482.    double **A = MATRIX (1, d1, 1, d1, double);
  483.    double **BT = alpha+d1;
  484.    double **C = MATRIX (1, d2, 1, d2, double);
  485.    double **B = MATRIX (1, d1, 1, d2, double);
  486.    double *K1 = VECTOR (1, d1, double);
  487.    double *K2 = beta + d1;
  488.  
  489.    /* Transfer matrixes C and B */
  490.    for (i=1; i<=d2; i++)
  491.       for (j=1; j<=d2; j++)
  492.         C[i][j] = alpha[i+d1][j+d1];
  493.  
  494.    for (i=1; i<=d1; i++)
  495.       for (j=1; j<=d2; j++)
  496.      B[i][j] = alpha [i][j+d1];
  497.  
  498.    /* Invert C in place */
  499.    if (! gaussj2_check (C, d2, (double **) 0, 0))
  500.       {  
  501.       error_message ("Can not invert matrix");
  502.       bail_out (1);
  503.       }  
  504.  
  505.    /* Multiply it on the right by B */
  506.    matrix_prod (B, C, d1, d2, d2, B);
  507.  
  508.    /* Now multiply it into A */
  509.    matrix_prod (B, BT, d1, d2, d1, A);
  510.    
  511.    /* Change the sign and add the elements of alpha */
  512.    for (i=1; i<=d1; i++)
  513.       for (j=1; j<=d1; j++)
  514.        A[i][j] = alpha[i][j] - A[i][j];
  515.  
  516.    /* Now do the same thing for the goal vector */
  517.    matrix_vector_prod (B, K2, d1, d2, K1);
  518.    for (i=1; i<=d1; i++)
  519.       K1[i] = beta[i] - K1[i];
  520.  
  521.    /* Now output the two matrices */
  522.    fprintf (stderr, "\nGOAL VECTOR\n");
  523.    fprint_vector ("%10.3e ", stderr, K1, 1, d1);
  524.    fprintf (stderr, "\nNORMAL MATRIX\n");
  525.    fprint_matrix ("%10.3e ", stderr, A, 1, d1, 1, d1);
  526.  
  527.    /* Free the temporaries */
  528.    FREE (A, 1);
  529.    FREE (C, 1);
  530.    FREE (B, 1);
  531.    FREE (K1, 1);
  532.    }
  533.  
  534.    
  535.    
  536.