lmlec_core.c

levenberg marquit算法的.C源码

     * fit in the L1 cache. For such problems, a cache-efficient blocking scheme
     * is preferable. On the other hand, the straightforward algorithm is faster
     * on small problems since in this case it avoids the overheads of blocking.
     */

    for(i=0; i<n; ++i){
      aux1=jac+i*m;
      aux2=jacjac+i*mm;
      for(j=0; j<mm; ++j){
        for(l=0, sum=0.0; l<m; ++l)
          sum+=aux1[l]*Z[l*mm+j]; // sum+=jac[i*m+l]*Z[l*mm+j];

        aux2[j]=sum; // jacjac[i*mm+j]=sum;
      }
    }
  }
  else{ // this is a large problem
    /* Cache efficient computation of jac*Z based on blocking
     */
#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
    register int jj, ll;

    for(jj=0; jj<mm; jj+=__BLOCKSZ__){
      for(i=0; i<n; ++i){
        aux1=jacjac+i*mm;
        for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j)
          aux1[j]=0.0; //jacjac[i*mm+j]=0.0;
      }

      for(ll=0; ll<m; ll+=__BLOCKSZ__){
        for(i=0; i<n; ++i){
          aux1=jacjac+i*mm; aux2=jac+i*m;
          for(j=jj; j<__MIN__(jj+__BLOCKSZ__, mm); ++j){
            sum=0.0;
            for(l=ll; l<__MIN__(ll+__BLOCKSZ__, m); ++l)
              sum+=aux2[l]*Z[l*mm+j]; //jac[i*m+l]*Z[l*mm+j];
            aux1[j]+=sum; //jacjac[i*mm+j]+=sum;
          }
        }
      }
    }
  }
}
#undef __MIN__


/* 
 * This function is similar to LEVMAR_DER except that the minimization
 * is performed subject to the linear constraints A p=b, A is kxm, b kx1
 *
 * This function requires an analytic jacobian. In case the latter is unavailable,
 * use LEVMAR_LEC_DIF() bellow
 *
 */
int LEVMAR_LEC_DER(
  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
  void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),  /* function to evaluate the jacobian \part x / \part p */ 
  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
  LM_REAL *x,         /* I: measurement vector */
  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
  int n,              /* I: measurement vector dimension */
  LM_REAL *A,         /* I: constraints matrix, kxm */
  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
  int k,              /* I: number of contraints (i.e. A's #rows) */
  int itmax,          /* I: maximum number of iterations */
  LM_REAL opts[4],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3]. Respectively the scale factor for initial \mu,
                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2. Set to NULL for defaults to be used
                       */
  LM_REAL info[LM_INFO_SZ],
					           /* O: information regarding the minimization. Set to NULL if don't care
                      * info[0]= ||e||_2 at initial p.
                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
                      * info[5]= # iterations,
                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu 
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      * info[7]= # function evaluations
                      * info[8]= # jacobian evaluations
                      */
  LM_REAL *work,     /* working memory, allocate if NULL */
  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func & jacf.
                      * Set to NULL if not needed
                      */
{
  struct LMLEC_DATA data;
  LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
  int mm, ret;
  register int i, j;
  register LM_REAL tmp;
  LM_REAL locinfo[LM_INFO_SZ];

  if(!jacf){
    fprintf(stderr, RCAT("No function specified for computing the jacobian in ", LEVMAR_LEC_DER)
      RCAT("().\nIf no such function is available, use ", LEVMAR_LEC_DIF) RCAT("() rather than ", LEVMAR_LEC_DER) "()\n");
    exit(1);
  }

  mm=m-k;

  ptr=(LM_REAL *)malloc((2*m + m*mm + n*m + mm)*sizeof(LM_REAL));
  if(!ptr){
    fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): memory allocation request failed\n"));
    exit(1);
  }
  data.p=p;
  p0=ptr;
  data.c=p0+m;
  data.Z=Z=data.c+m;
  data.jac=data.Z+m*mm;
  pp=data.jac+n*m;
  data.ncnstr=k;
  data.func=func;
  data.jacf=jacf;
  data.adata=adata;

  LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z

  /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
   * Due to orthogonality, Z^T Z = I and the last equation
   * becomes pp=Z^T*(p-c). Also, save the starting p in p0
   */
  for(i=0; i<m; ++i){
    p0[i]=p[i];
    p[i]-=data.c[i];
  }

  /* Z^T*(p-c) */
  for(i=0; i<mm; ++i){
    for(j=0, tmp=0.0; j<m; ++j)
      tmp+=Z[j*mm+i]*p[j];
    pp[i]=tmp;
  }

  /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
  for(i=0; i<m; ++i){
    Zimm=Z+i*mm;
    for(j=0, tmp=data.c[i]; j<mm; ++j)
      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
    if(FABS(tmp-p0[i])>CNST(1E-03))
      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DER) "()! [%.10g reset to %.10g]\n",
                      i, p0[i], tmp);
  }

  if(!info) info=locinfo; /* make sure that LEVMAR_DER() is called with non-null info */
  /* note that covariance computation is not requested from LEVMAR_DER() */
  ret=LEVMAR_DER(LMLEC_FUNC, LMLEC_JACF, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);

  /* p=c + Z*pp */
  for(i=0; i<m; ++i){
    Zimm=Z+i*mm;
    for(j=0, tmp=data.c[i]; j<mm; ++j)
      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
    p[i]=tmp;
  }

  /* compute the covariance from the jacobian in data.jac */
  if(covar){
    TRANS_MAT_MAT_MULT(data.jac, covar, n, m); /* covar = J^T J */
    LEVMAR_COVAR(covar, covar, info[1], m, n);
  }

  free(ptr);

  return ret;
}

/* Similar to the LEVMAR_LEC_DER() function above, except that the jacobian is approximated
 * with the aid of finite differences (forward or central, see the comment for the opts argument)
 */
int LEVMAR_LEC_DIF(
  void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
  LM_REAL *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
  LM_REAL *x,         /* I: measurement vector */
  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
  int n,              /* I: measurement vector dimension */
  LM_REAL *A,         /* I: constraints matrix, kxm */
  LM_REAL *b,         /* I: right hand constraints vector, kx1 */
  int k,              /* I: number of contraints (i.e. A's #rows) */
  int itmax,          /* I: maximum number of iterations */
  LM_REAL opts[5],    /* I: opts[0-3] = minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \delta]. Respectively the
                       * scale factor for initial \mu, stopping thresholds for ||J^T e||_inf, ||Dp||_2 and ||e||_2 and
                       * the step used in difference approximation to the jacobian. Set to NULL for defaults to be used.
                       * If \delta<0, the jacobian is approximated with central differences which are more accurate
                       * (but slower!) compared to the forward differences employed by default. 
                       */
  LM_REAL info[LM_INFO_SZ],
					           /* O: information regarding the minimization. Set to NULL if don't care
                      * info[0]= ||e||_2 at initial p.
                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
                      * info[5]= # iterations,
                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu 
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      * info[7]= # function evaluations
                      * info[8]= # jacobian evaluations
                      */
  LM_REAL *work,     /* working memory, allocate if NULL */
  LM_REAL *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
  void *adata)       /* pointer to possibly additional data, passed uninterpreted to func.
                      * Set to NULL if not needed
                      */
{
  struct LMLEC_DATA data;
  LM_REAL *ptr, *Z, *pp, *p0, *Zimm; /* Z is mxmm */
  int mm, ret;
  register int i, j;
  register LM_REAL tmp;
  LM_REAL locinfo[LM_INFO_SZ];

  mm=m-k;

  ptr=(LM_REAL *)malloc((2*m + m*mm + mm)*sizeof(LM_REAL));
  if(!ptr){
    fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
    exit(1);
  }
  data.p=p;
  p0=ptr;
  data.c=p0+m;
  data.Z=Z=data.c+m;
  data.jac=NULL;
  pp=data.Z+m*mm;
  data.ncnstr=k;
  data.func=func;
  data.jacf=NULL;
  data.adata=adata;

  LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z

  /* compute pp s.t. p = c + Z*pp or (Z^T Z)*pp=Z^T*(p-c)
   * Due to orthogonality, Z^T Z = I and the last equation
   * becomes pp=Z^T*(p-c). Also, save the starting p in p0
   */
  for(i=0; i<m; ++i){
    p0[i]=p[i];
    p[i]-=data.c[i];
  }

  /* Z^T*(p-c) */
  for(i=0; i<mm; ++i){
    for(j=0, tmp=0.0; j<m; ++j)
      tmp+=Z[j*mm+i]*p[j];
    pp[i]=tmp;
  }

  /* compute the p corresponding to pp (i.e. c + Z*pp) and compare with p0 */
  for(i=0; i<m; ++i){
    Zimm=Z+i*mm;
    for(j=0, tmp=data.c[i]; j<mm; ++j)
      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
    if(FABS(tmp-p0[i])>CNST(1E-03))
      fprintf(stderr, RCAT("Warning: component %d of starting point not feasible in ", LEVMAR_LEC_DIF) "()! [%.10g reset to %.10g]\n",
                      i, p0[i], tmp);
  }

  if(!info) info=locinfo; /* make sure that LEVMAR_DIF() is called with non-null info */
  /* note that covariance computation is not requested from LEVMAR_DIF() */
  ret=LEVMAR_DIF(LMLEC_FUNC, pp, x, mm, n, itmax, opts, info, work, NULL, (void *)&data);

  /* p=c + Z*pp */
  for(i=0; i<m; ++i){
    Zimm=Z+i*mm;
    for(j=0, tmp=data.c[i]; j<mm; ++j)
      tmp+=Zimm[j]*pp[j]; // tmp+=Z[i*mm+j]*pp[j];
    p[i]=tmp;
  }

  /* compute the jacobian with finite differences and use it to estimate the covariance */
  if(covar){
    LM_REAL *hx, *wrk, *jac;

    hx=(LM_REAL *)malloc((2*n+n*m)*sizeof(LM_REAL));
    if(!work){
      fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
      exit(1);
    }

    wrk=hx+n;
    jac=wrk+n;

    (*func)(p, hx, m, n, adata); /* evaluate function at p */
    FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the jacobian at p */
    TRANS_MAT_MAT_MULT(jac, covar, n, m); /* covar = J^T J */
    LEVMAR_COVAR(covar, covar, info[1], m, n);
    free(hx);
  }

  free(ptr);

  return ret;
}

/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
#undef LMLEC_DATA
#undef LMLEC_ELIM
#undef LMLEC_FUNC
#undef LMLEC_JACF
#undef FDIF_FORW_JAC_APPROX
#undef LEVMAR_COVAR
#undef TRANS_MAT_MAT_MULT
#undef LEVMAR_LEC_DER
#undef LEVMAR_LEC_DIF
#undef LEVMAR_DER
#undef LEVMAR_DIF

#undef GEQP3
#undef ORGQR
#undef TRTRI