mirror of
https://github.com/GoldenCheetah/GoldenCheetah.git
synced 2026-04-15 05:32:21 +00:00
Remove levmar dependency
It is not being used
This commit is contained in:
Alejandro Martinez
@@ -1,75 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Solution of linear systems involved in the Levenberg - Marquardt
|
||||
// minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/********************************************************************************
|
||||
* LAPACK-based implementations for various linear system solvers. The same core
|
||||
* code is used with appropriate #defines to derive single and double precision
|
||||
* solver versions, see also Axb_core.c
|
||||
********************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "misc.h"
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
#define LM_CNST(x) (x)
|
||||
#ifndef HAVE_LAPACK
|
||||
#include <float.h>
|
||||
#define LM_REAL_EPSILON DBL_EPSILON
|
||||
#endif
|
||||
|
||||
#include "Axb_core.c"
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_CNST
|
||||
#undef LM_REAL_EPSILON
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
#ifndef HAVE_LAPACK
|
||||
#define LM_REAL_EPSILON FLT_EPSILON
|
||||
#endif
|
||||
|
||||
#include "Axb_core.c"
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#undef LM_REAL_EPSILON
|
||||
#endif /* LM_SNGL_PREC */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,49 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef _COMPILER_H_
|
||||
#define _COMPILER_H_
|
||||
|
||||
/* note: intel's icc defines both __ICC & __INTEL_COMPILER.
|
||||
* Also, some compilers other than gcc define __GNUC__,
|
||||
* therefore gcc should be checked last
|
||||
*/
|
||||
#ifdef _MSC_VER
|
||||
#define inline __inline // MSVC
|
||||
#elif !defined(__ICC) && !defined(__INTEL_COMPILER) && !defined(__GNUC__)
|
||||
#define inline // other than MSVC, ICC, GCC: define empty
|
||||
#endif
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define LM_FINITE _finite // MSVC
|
||||
#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)
|
||||
#define LM_FINITE finite // ICC, GCC
|
||||
#else
|
||||
#define LM_FINITE finite // other than MSVC, ICC, GCC, let's hope this will work
|
||||
#endif
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define LM_ISINF(x) (!_finite(x) && !_isnan(x)) // MSVC
|
||||
#elif defined(__ICC) || defined(__INTEL_COMPILER) || defined(__GNUC__)
|
||||
#define LM_ISINF(x) isinf(x) // ICC, GCC
|
||||
#else
|
||||
#define LM_ISINF(x) isinf(x) // other than MSVC, ICC, GCC, let's hope this will work
|
||||
#endif
|
||||
|
||||
#endif /* _COMPILER_H_ */
|
||||
@@ -1,398 +0,0 @@
|
||||
/*
|
||||
////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Prototypes and definitions for the Levenberg - Marquardt minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
////////////////////////////////////////////////////////////////////////////////////
|
||||
*/
|
||||
|
||||
#ifndef _LEVMAR_H_
|
||||
#define _LEVMAR_H_
|
||||
|
||||
/************************************* Start of configuration options *************************************/
|
||||
/* Note that when compiling with CMake, this configuration section is automatically generated
|
||||
* based on the user's input, see levmar.h.in
|
||||
*/
|
||||
|
||||
/* specifies whether to use LAPACK or not. Using LAPACK is strongly recommended */
|
||||
//#define HAVE_LAPACK
|
||||
|
||||
/* specifies whether the PLASMA parallel library for multicore CPUs is available */
|
||||
/* #undef HAVE_PLASMA */
|
||||
|
||||
/* to avoid the overhead of repeated mallocs(), routines in Axb.c can be instructed to
|
||||
* retain working memory between calls. Such a choice, however, renders these routines
|
||||
* non-reentrant and is not safe in a shared memory multiprocessing environment.
|
||||
* Bellow, an attempt is made to issue a warning if this option is turned on and OpenMP
|
||||
* is being used (note that this will work only if omp.h is included before levmar.h)
|
||||
*/
|
||||
#define LINSOLVERS_RETAIN_MEMORY
|
||||
#if (defined(_OPENMP))
|
||||
# ifdef LINSOLVERS_RETAIN_MEMORY
|
||||
# ifdef _MSC_VER
|
||||
# pragma message("LINSOLVERS_RETAIN_MEMORY is not safe in a multithreaded environment and should be turned off!")
|
||||
# else
|
||||
# warning LINSOLVERS_RETAIN_MEMORY is not safe in a multithreaded environment and should be turned off!
|
||||
# endif /* _MSC_VER */
|
||||
# endif /* LINSOLVERS_RETAIN_MEMORY */
|
||||
#endif /* _OPENMP */
|
||||
|
||||
/* specifies whether double precision routines will be compiled or not */
|
||||
#define LM_DBL_PREC
|
||||
/* specifies whether single precision routines will be compiled or not */
|
||||
#define LM_SNGL_PREC
|
||||
|
||||
/****************** End of configuration options, no changes necessary beyond this point ******************/
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* work arrays size for ?levmar_der and ?levmar_dif functions.
|
||||
* should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
|
||||
*/
|
||||
#define LM_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
|
||||
#define LM_DIF_WORKSZ(npar, nmeas) (4*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
|
||||
|
||||
/* work arrays size for ?levmar_bc_der and ?levmar_bc_dif functions.
|
||||
* should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
|
||||
*/
|
||||
#define LM_BC_DER_WORKSZ(npar, nmeas) (2*(nmeas) + 4*(npar) + (nmeas)*(npar) + (npar)*(npar))
|
||||
#define LM_BC_DIF_WORKSZ(npar, nmeas) LM_BC_DER_WORKSZ((npar), (nmeas)) /* LEVMAR_BC_DIF currently implemented using LEVMAR_BC_DER()! */
|
||||
|
||||
/* work arrays size for ?levmar_lec_der and ?levmar_lec_dif functions.
|
||||
* should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
|
||||
*/
|
||||
#define LM_LEC_DER_WORKSZ(npar, nmeas, nconstr) LM_DER_WORKSZ((npar)-(nconstr), (nmeas))
|
||||
#define LM_LEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_DIF_WORKSZ((npar)-(nconstr), (nmeas))
|
||||
|
||||
/* work arrays size for ?levmar_blec_der and ?levmar_blec_dif functions.
|
||||
* should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
|
||||
*/
|
||||
#define LM_BLEC_DER_WORKSZ(npar, nmeas, nconstr) LM_LEC_DER_WORKSZ((npar), (nmeas)+(npar), (nconstr))
|
||||
#define LM_BLEC_DIF_WORKSZ(npar, nmeas, nconstr) LM_LEC_DIF_WORKSZ((npar), (nmeas)+(npar), (nconstr))
|
||||
|
||||
/* work arrays size for ?levmar_bleic_der and ?levmar_bleic_dif functions.
|
||||
* should be multiplied by sizeof(double) or sizeof(float) to be converted to bytes
|
||||
*/
|
||||
#define LM_BLEIC_DER_WORKSZ(npar, nmeas, nconstr1, nconstr2) LM_BLEC_DER_WORKSZ((npar)+(nconstr2), (nmeas)+(nconstr2), (nconstr1)+(nconstr2))
|
||||
#define LM_BLEIC_DIF_WORKSZ(npar, nmeas, nconstr1, nconstr2) LM_BLEC_DIF_WORKSZ((npar)+(nconstr2), (nmeas)+(nconstr2), (nconstr1)+(nconstr2))
|
||||
|
||||
#define LM_OPTS_SZ 5 /* max(4, 5) */
|
||||
#define LM_INFO_SZ 10
|
||||
#define LM_ERROR -1
|
||||
#define LM_INIT_MU 1E-03
|
||||
#define LM_STOP_THRESH 1E-17
|
||||
#define LM_DIFF_DELTA 1E-06
|
||||
#define LM_VERSION "2.6 (November 2011)"
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision LM, with & without Jacobian */
|
||||
/* unconstrained minimization */
|
||||
extern int dlevmar_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, int itmax, double *opts,
|
||||
double *info, double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, int itmax, double *opts,
|
||||
double *info, double *work, double *covar, void *adata);
|
||||
|
||||
/* box-constrained minimization */
|
||||
extern int dlevmar_bc_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *dscl,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_bc_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *dscl,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
/* linear equation constrained minimization */
|
||||
extern int dlevmar_lec_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *A, double *b, int k,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_lec_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *A, double *b, int k,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
/* box & linear equation constrained minimization */
|
||||
extern int dlevmar_blec_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_blec_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *A, double *b, int k, double *wghts,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
/* box, linear equations & inequalities constrained minimization */
|
||||
extern int dlevmar_bleic_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub,
|
||||
double *A, double *b, int k1, double *C, double *d, int k2,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_bleic_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub,
|
||||
double *A, double *b, int k1, double *C, double *d, int k2,
|
||||
int itmax, double *opts, double *info, double *work, double *covar, void *adata);
|
||||
|
||||
/* box & linear inequality constraints */
|
||||
extern int dlevmar_blic_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *C, double *d, int k2,
|
||||
int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_blic_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *lb, double *ub, double *C, double *d, int k2,
|
||||
int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
|
||||
/* linear equation & inequality constraints */
|
||||
extern int dlevmar_leic_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *A, double *b, int k1, double *C, double *d, int k2,
|
||||
int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_leic_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *A, double *b, int k1, double *C, double *d, int k2,
|
||||
int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
|
||||
/* linear inequality constraints */
|
||||
extern int dlevmar_lic_der(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *C, double *d, int k2,
|
||||
int itmax, double opts[4], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
|
||||
extern int dlevmar_lic_dif(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *x, int m, int n, double *C, double *d, int k2,
|
||||
int itmax, double opts[5], double info[LM_INFO_SZ], double *work, double *covar, void *adata);
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision LM, with & without Jacobian */
|
||||
/* unconstrained minimization */
|
||||
extern int slevmar_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, int itmax, float *opts,
|
||||
float *info, float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, int itmax, float *opts,
|
||||
float *info, float *work, float *covar, void *adata);
|
||||
|
||||
/* box-constrained minimization */
|
||||
extern int slevmar_bc_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *dscl,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_bc_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *dscl,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
/* linear equation constrained minimization */
|
||||
extern int slevmar_lec_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *A, float *b, int k,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_lec_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *A, float *b, int k,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
/* box & linear equation constrained minimization */
|
||||
extern int slevmar_blec_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_blec_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *A, float *b, int k, float *wghts,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
/* box, linear equations & inequalities constrained minimization */
|
||||
extern int slevmar_bleic_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub,
|
||||
float *A, float *b, int k1, float *C, float *d, int k2,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_bleic_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub,
|
||||
float *A, float *b, int k1, float *C, float *d, int k2,
|
||||
int itmax, float *opts, float *info, float *work, float *covar, void *adata);
|
||||
|
||||
/* box & linear inequality constraints */
|
||||
extern int slevmar_blic_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *C, float *d, int k2,
|
||||
int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_blic_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *lb, float *ub, float *C, float *d, int k2,
|
||||
int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
|
||||
/* linear equality & inequality constraints */
|
||||
extern int slevmar_leic_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *A, float *b, int k1, float *C, float *d, int k2,
|
||||
int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_leic_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *A, float *b, int k1, float *C, float *d, int k2,
|
||||
int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
|
||||
/* linear inequality constraints */
|
||||
extern int slevmar_lic_der(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *C, float *d, int k2,
|
||||
int itmax, float opts[4], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
|
||||
extern int slevmar_lic_dif(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, float *C, float *d, int k2,
|
||||
int itmax, float opts[5], float info[LM_INFO_SZ], float *work, float *covar, void *adata);
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
/* linear system solvers */
|
||||
#ifdef HAVE_LAPACK
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
extern int dAx_eq_b_QR(double *A, double *B, double *x, int m);
|
||||
extern int dAx_eq_b_QRLS(double *A, double *B, double *x, int m, int n);
|
||||
extern int dAx_eq_b_Chol(double *A, double *B, double *x, int m);
|
||||
extern int dAx_eq_b_LU(double *A, double *B, double *x, int m);
|
||||
extern int dAx_eq_b_SVD(double *A, double *B, double *x, int m);
|
||||
extern int dAx_eq_b_BK(double *A, double *B, double *x, int m);
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
extern int sAx_eq_b_QR(float *A, float *B, float *x, int m);
|
||||
extern int sAx_eq_b_QRLS(float *A, float *B, float *x, int m, int n);
|
||||
extern int sAx_eq_b_Chol(float *A, float *B, float *x, int m);
|
||||
extern int sAx_eq_b_LU(float *A, float *B, float *x, int m);
|
||||
extern int sAx_eq_b_SVD(float *A, float *B, float *x, int m);
|
||||
extern int sAx_eq_b_BK(float *A, float *B, float *x, int m);
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#else /* no LAPACK */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
extern int dAx_eq_b_LU_noLapack(double *A, double *B, double *x, int n);
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
extern int sAx_eq_b_LU_noLapack(float *A, float *B, float *x, int n);
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
#ifdef HAVE_PLASMA
|
||||
#ifdef LM_DBL_PREC
|
||||
extern int dAx_eq_b_PLASMA_Chol(double *A, double *B, double *x, int m);
|
||||
#endif
|
||||
#ifdef LM_SNGL_PREC
|
||||
extern int sAx_eq_b_PLASMA_Chol(float *A, float *B, float *x, int m);
|
||||
#endif
|
||||
extern void levmar_PLASMA_setnbcores(int cores);
|
||||
#endif /* HAVE_PLASMA */
|
||||
|
||||
/* Jacobian verification, double & single precision */
|
||||
#ifdef LM_DBL_PREC
|
||||
extern void dlevmar_chkjac(
|
||||
void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
void (*jacf)(double *p, double *j, int m, int n, void *adata),
|
||||
double *p, int m, int n, void *adata, double *err);
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
extern void slevmar_chkjac(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
void (*jacf)(float *p, float *j, int m, int n, void *adata),
|
||||
float *p, int m, int n, void *adata, float *err);
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
/* miscellaneous: standard deviation, coefficient of determination (R2),
|
||||
* Pearson's correlation coefficient for best-fit parameters
|
||||
*/
|
||||
#ifdef LM_DBL_PREC
|
||||
extern double dlevmar_stddev( double *covar, int m, int i);
|
||||
extern double dlevmar_corcoef(double *covar, int m, int i, int j);
|
||||
extern double dlevmar_R2(void (*func)(double *p, double *hx, int m, int n, void *adata), double *p, double *x, int m, int n, void *adata);
|
||||
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
extern float slevmar_stddev( float *covar, int m, int i);
|
||||
extern float slevmar_corcoef(float *covar, int m, int i, int j);
|
||||
extern float slevmar_R2(void (*func)(float *p, float *hx, int m, int n, void *adata), float *p, float *x, int m, int n, void *adata);
|
||||
|
||||
extern void slevmar_locscale(
|
||||
void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *x, int m, int n, void *adata,
|
||||
int howto, float locscl[2], float **residptr);
|
||||
|
||||
extern int slevmar_outlid(float *r, int n, float thresh, float ls[2], char *outlmap);
|
||||
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _LEVMAR_H_ */
|
||||
@@ -1,83 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/********************************************************************************
|
||||
* Levenberg-Marquardt nonlinear minimization. The same core code is used with
|
||||
* appropriate #defines to derive single and double precision versions, see
|
||||
* also lm_core.c
|
||||
********************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "compiler.h"
|
||||
#include "misc.h"
|
||||
|
||||
#define EPSILON 1E-12
|
||||
#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define LM_REAL_MAX FLT_MAX
|
||||
#define LM_REAL_MIN -FLT_MAX
|
||||
#define LM_REAL_EPSILON FLT_EPSILON
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "lm_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef LM_REAL_MIN
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_REAL_MAX DBL_MAX
|
||||
#define LM_REAL_MIN -DBL_MAX
|
||||
#define LM_REAL_EPSILON DBL_EPSILON
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "lm_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef LM_REAL_MIN
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
@@ -1,11 +0,0 @@
|
||||
#ifndef _DEPR_LM_H_
|
||||
#define _DEPR_LM_H_
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("lm.h is deprecated, please use levmar.h instead!")
|
||||
#else
|
||||
#error lm.h is deprecated, please use levmar.h instead!
|
||||
#endif /* _MSC_VER */
|
||||
|
||||
#endif /* _DEPR_LM_H_ */
|
||||
|
||||
@@ -1,858 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef LM_REAL // not included by lm.c
|
||||
#error This file should not be compiled directly!
|
||||
#endif
|
||||
|
||||
|
||||
/* precision-specific definitions */
|
||||
#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
|
||||
#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)
|
||||
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
|
||||
#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
|
||||
#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
|
||||
#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
|
||||
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
|
||||
#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
|
||||
#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
|
||||
#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
|
||||
#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
|
||||
#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
|
||||
#else
|
||||
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
#ifdef HAVE_PLASMA
|
||||
#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* This function seeks the parameter vector p that best describes the measurements vector x.
|
||||
* More precisely, given a vector function func : R^m --> R^n with n>=m,
|
||||
* it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
|
||||
* e=x-func(p) is minimized.
|
||||
*
|
||||
* This function requires an analytic Jacobian. In case the latter is unavailable,
|
||||
* use LEVMAR_DIF() bellow
|
||||
*
|
||||
* Returns the number of iterations (>=0) if successful, LM_ERROR if failed
|
||||
*
|
||||
* For more details, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on
|
||||
* non-linear least squares at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
|
||||
*/
|
||||
|
||||
int LEVMAR_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_DER_WORKSZ() reals large, allocated 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
|
||||
*/
|
||||
{
|
||||
register int i, j, k, l;
|
||||
int worksz, freework=0, issolved;
|
||||
/* temp work arrays */
|
||||
LM_REAL *e, /* nx1 */
|
||||
*hx, /* \hat{x}_i, nx1 */
|
||||
*jacTe, /* J^T e_i mx1 */
|
||||
*jac, /* nxm */
|
||||
*jacTjac, /* mxm */
|
||||
*Dp, /* mx1 */
|
||||
*diag_jacTjac, /* diagonal of J^T J, mx1 */
|
||||
*pDp; /* p + Dp, mx1 */
|
||||
|
||||
register LM_REAL mu, /* damping constant */
|
||||
tmp; /* mainly used in matrix & vector multiplications */
|
||||
LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
|
||||
LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
|
||||
LM_REAL tau, eps1, eps2, eps2_sq, eps3;
|
||||
LM_REAL init_p_eL2;
|
||||
int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
|
||||
const int nm=n*m;
|
||||
int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
|
||||
|
||||
mu=jacTe_inf=0.0; /* -Wall */
|
||||
|
||||
if(n<m){
|
||||
fprintf(stderr, LCAT(LEVMAR_DER, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!jacf){
|
||||
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_DER)
|
||||
RCAT("().\nIf no such function is available, use ", LEVMAR_DIF) RCAT("() rather than ", LEVMAR_DER) "()\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(opts){
|
||||
tau=opts[0];
|
||||
eps1=opts[1];
|
||||
eps2=opts[2];
|
||||
eps2_sq=opts[2]*opts[2];
|
||||
eps3=opts[3];
|
||||
}
|
||||
else{ // use default values
|
||||
tau=LM_CNST(LM_INIT_MU);
|
||||
eps1=LM_CNST(LM_STOP_THRESH);
|
||||
eps2=LM_CNST(LM_STOP_THRESH);
|
||||
eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
|
||||
eps3=LM_CNST(LM_STOP_THRESH);
|
||||
}
|
||||
|
||||
if(!work){
|
||||
worksz=LM_DER_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
|
||||
work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
|
||||
if(!work){
|
||||
fprintf(stderr, LCAT(LEVMAR_DER, "(): memory allocation request failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
freework=1;
|
||||
}
|
||||
|
||||
/* set up work arrays */
|
||||
e=work;
|
||||
hx=e + n;
|
||||
jacTe=hx + n;
|
||||
jac=jacTe + m;
|
||||
jacTjac=jac + nm;
|
||||
Dp=jacTjac + m*m;
|
||||
diag_jacTjac=Dp + m;
|
||||
pDp=diag_jacTjac + m;
|
||||
|
||||
/* compute e=x - f(p) and its L2 norm */
|
||||
(*func)(p, hx, m, n, adata); nfev=1;
|
||||
/* ### e=x-hx, p_eL2=||e|| */
|
||||
#if 1
|
||||
p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
|
||||
#else
|
||||
for(i=0, p_eL2=0.0; i<n; ++i){
|
||||
e[i]=tmp=x[i]-hx[i];
|
||||
p_eL2+=tmp*tmp;
|
||||
}
|
||||
#endif
|
||||
init_p_eL2=p_eL2;
|
||||
if(!LM_FINITE(p_eL2)) stop=7;
|
||||
|
||||
for(k=0; k<itmax && !stop; ++k){
|
||||
/* Note that p and e have been updated at a previous iteration */
|
||||
|
||||
if(p_eL2<=eps3){ /* error is small */
|
||||
stop=6;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
|
||||
* Since J^T J is symmetric, its computation can be sped up by computing
|
||||
* only its upper triangular part and copying it to the lower part
|
||||
*/
|
||||
|
||||
(*jacf)(p, jac, m, n, adata); ++njev;
|
||||
|
||||
/* J^T J, J^T e */
|
||||
if(nm<__BLOCKSZ__SQ){ // this is a small problem
|
||||
/* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.
|
||||
* Thus, the product J^T J can be computed using an outer loop for
|
||||
* l that adds J_li*J_lj to each element ij of the result. Note that
|
||||
* with this scheme, the accesses to J and JtJ are always along rows,
|
||||
* therefore induces less cache misses compared to the straightforward
|
||||
* algorithm for computing the product (i.e., l loop is innermost one).
|
||||
* A similar scheme applies to the computation of J^T e.
|
||||
* However, for large minimization problems (i.e., involving a large number
|
||||
* of unknowns and measurements) for which J/J^T J rows are too large to
|
||||
* fit in the L1 cache, even this scheme incures many cache misses. In
|
||||
* such cases, a cache-efficient blocking scheme is preferable.
|
||||
*
|
||||
* Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
|
||||
* performance problem.
|
||||
*
|
||||
* Note that the non-blocking algorithm is faster on small
|
||||
* problems since in this case it avoids the overheads of blocking.
|
||||
*/
|
||||
|
||||
/* looping downwards saves a few computations */
|
||||
register int l;
|
||||
register LM_REAL alpha, *jaclm, *jacTjacim;
|
||||
|
||||
for(i=m*m; i-->0; )
|
||||
jacTjac[i]=0.0;
|
||||
for(i=m; i-->0; )
|
||||
jacTe[i]=0.0;
|
||||
|
||||
for(l=n; l-->0; ){
|
||||
jaclm=jac+l*m;
|
||||
for(i=m; i-->0; ){
|
||||
jacTjacim=jacTjac+i*m;
|
||||
alpha=jaclm[i]; //jac[l*m+i];
|
||||
for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
|
||||
jacTjacim[j]+=jaclm[j]*alpha; //jacTjac[i*m+j]+=jac[l*m+j]*alpha
|
||||
|
||||
/* J^T e */
|
||||
jacTe[i]+=alpha*e[l];
|
||||
}
|
||||
}
|
||||
|
||||
for(i=m; i-->0; ) /* copy to upper part */
|
||||
for(j=i+1; j<m; ++j)
|
||||
jacTjac[i*m+j]=jacTjac[j*m+i];
|
||||
|
||||
}
|
||||
else{ // this is a large problem
|
||||
/* Cache efficient computation of J^T J based on blocking
|
||||
*/
|
||||
LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
|
||||
|
||||
/* cache efficient computation of J^T e */
|
||||
for(i=0; i<m; ++i)
|
||||
jacTe[i]=0.0;
|
||||
|
||||
for(i=0; i<n; ++i){
|
||||
register LM_REAL *jacrow;
|
||||
|
||||
for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
|
||||
jacTe[l]+=jacrow[l]*tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/* Compute ||J^T e||_inf and ||p||^2 */
|
||||
for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
|
||||
if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
|
||||
|
||||
diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
|
||||
p_L2+=p[i]*p[i];
|
||||
}
|
||||
//p_L2=sqrt(p_L2);
|
||||
|
||||
#if 0
|
||||
if(!(k%100)){
|
||||
printf("Current estimate: ");
|
||||
for(i=0; i<m; ++i)
|
||||
printf("%.9g ", p[i]);
|
||||
printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* check for convergence */
|
||||
if((jacTe_inf <= eps1)){
|
||||
Dp_L2=0.0; /* no increment for p in this case */
|
||||
stop=1;
|
||||
break;
|
||||
}
|
||||
|
||||
/* compute initial damping factor */
|
||||
if(k==0){
|
||||
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
|
||||
if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
|
||||
mu=tau*tmp;
|
||||
}
|
||||
|
||||
/* determine increment using adaptive damping */
|
||||
while(1){
|
||||
/* augment normal equations */
|
||||
for(i=0; i<m; ++i)
|
||||
jacTjac[i*m+i]+=mu;
|
||||
|
||||
/* solve augmented equations */
|
||||
#ifdef HAVE_LAPACK
|
||||
/* 7 alternatives are available: LU, Cholesky + Cholesky with PLASMA, LDLt, 2 variants of QR decomposition and SVD.
|
||||
* For matrices with dimensions of at least a few hundreds, the PLASMA implementation of Cholesky is the fastest.
|
||||
* From the serial solvers, Cholesky is the fastest but might occasionally be inapplicable due to numerical round-off;
|
||||
* QR is slower but more robust; SVD is the slowest but most robust; LU is quite robust but
|
||||
* slower than LDLt; LDLt offers a good tradeoff between robustness and speed
|
||||
*/
|
||||
|
||||
issolved=AX_EQ_B_BK(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_BK;
|
||||
//issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
|
||||
//issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;
|
||||
#ifdef HAVE_PLASMA
|
||||
//issolved=AX_EQ_B_PLASMA_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_PLASMA_CHOL;
|
||||
#endif
|
||||
//issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;
|
||||
//issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;
|
||||
//issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;
|
||||
|
||||
#else
|
||||
/* use the LU included with levmar */
|
||||
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
if(issolved){
|
||||
/* compute p's new estimate and ||Dp||^2 */
|
||||
for(i=0, Dp_L2=0.0; i<m; ++i){
|
||||
pDp[i]=p[i] + (tmp=Dp[i]);
|
||||
Dp_L2+=tmp*tmp;
|
||||
}
|
||||
//Dp_L2=sqrt(Dp_L2);
|
||||
|
||||
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
|
||||
//if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
|
||||
stop=2;
|
||||
break;
|
||||
}
|
||||
|
||||
if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
|
||||
//if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
|
||||
stop=4;
|
||||
break;
|
||||
}
|
||||
|
||||
(*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
|
||||
/* compute ||e(pDp)||_2 */
|
||||
/* ### hx=x-hx, pDp_eL2=||hx|| */
|
||||
#if 1
|
||||
pDp_eL2=LEVMAR_L2NRMXMY(hx, x, hx, n);
|
||||
#else
|
||||
for(i=0, pDp_eL2=0.0; i<n; ++i){
|
||||
hx[i]=tmp=x[i]-hx[i];
|
||||
pDp_eL2+=tmp*tmp;
|
||||
}
|
||||
#endif
|
||||
if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
|
||||
* This check makes sure that the inner loop does not run indefinitely.
|
||||
* Thanks to Steve Danauskas for reporting such cases
|
||||
*/
|
||||
stop=7;
|
||||
break;
|
||||
}
|
||||
|
||||
for(i=0, dL=0.0; i<m; ++i)
|
||||
dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
|
||||
|
||||
dF=p_eL2-pDp_eL2;
|
||||
|
||||
if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
|
||||
tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
|
||||
tmp=LM_CNST(1.0)-tmp*tmp*tmp;
|
||||
mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
|
||||
nu=2;
|
||||
|
||||
for(i=0 ; i<m; ++i) /* update p's estimate */
|
||||
p[i]=pDp[i];
|
||||
|
||||
for(i=0; i<n; ++i) /* update e and ||e||_2 */
|
||||
e[i]=hx[i];
|
||||
p_eL2=pDp_eL2;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* if this point is reached, either the linear system could not be solved or
|
||||
* the error did not reduce; in any case, the increment must be rejected
|
||||
*/
|
||||
|
||||
mu*=nu;
|
||||
nu2=nu<<1; // 2*nu;
|
||||
if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
|
||||
stop=5;
|
||||
break;
|
||||
}
|
||||
nu=nu2;
|
||||
|
||||
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
|
||||
jacTjac[i*m+i]=diag_jacTjac[i];
|
||||
} /* inner loop */
|
||||
}
|
||||
|
||||
if(k>=itmax) stop=3;
|
||||
|
||||
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
|
||||
jacTjac[i*m+i]=diag_jacTjac[i];
|
||||
|
||||
if(info){
|
||||
info[0]=init_p_eL2;
|
||||
info[1]=p_eL2;
|
||||
info[2]=jacTe_inf;
|
||||
info[3]=Dp_L2;
|
||||
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
|
||||
if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
|
||||
info[4]=mu/tmp;
|
||||
info[5]=(LM_REAL)k;
|
||||
info[6]=(LM_REAL)stop;
|
||||
info[7]=(LM_REAL)nfev;
|
||||
info[8]=(LM_REAL)njev;
|
||||
info[9]=(LM_REAL)nlss;
|
||||
}
|
||||
|
||||
/* covariance matrix */
|
||||
if(covar){
|
||||
LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
|
||||
}
|
||||
|
||||
if(freework) free(work);
|
||||
|
||||
#ifdef LINSOLVERS_RETAIN_MEMORY
|
||||
if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
|
||||
#endif
|
||||
|
||||
return (stop!=4 && stop!=7)? k : LM_ERROR;
|
||||
}
|
||||
|
||||
|
||||
/* Secant version of the LEVMAR_DER() function above: the Jacobian is approximated with
|
||||
* the aid of finite differences (forward or central, see the comment for the opts argument)
|
||||
*/
|
||||
int LEVMAR_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
int itmax, /* I: maximum number of iterations */
|
||||
LM_REAL opts[5], /* I: opts[0-4] = 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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_DIF_WORKSZ() reals large, allocated 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
|
||||
*/
|
||||
{
|
||||
register int i, j, k, l;
|
||||
int worksz, freework=0, issolved;
|
||||
/* temp work arrays */
|
||||
LM_REAL *e, /* nx1 */
|
||||
*hx, /* \hat{x}_i, nx1 */
|
||||
*jacTe, /* J^T e_i mx1 */
|
||||
*jac, /* nxm */
|
||||
*jacTjac, /* mxm */
|
||||
*Dp, /* mx1 */
|
||||
*diag_jacTjac, /* diagonal of J^T J, mx1 */
|
||||
*pDp, /* p + Dp, mx1 */
|
||||
*wrk, /* nx1 */
|
||||
*wrk2; /* nx1, used only for holding a temporary e vector and when differentiating with central differences */
|
||||
|
||||
int using_ffdif=1;
|
||||
|
||||
register LM_REAL mu, /* damping constant */
|
||||
tmp; /* mainly used in matrix & vector multiplications */
|
||||
LM_REAL p_eL2, jacTe_inf, pDp_eL2; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
|
||||
LM_REAL p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
|
||||
LM_REAL tau, eps1, eps2, eps2_sq, eps3, delta;
|
||||
LM_REAL init_p_eL2;
|
||||
int nu, nu2, stop=0, nfev, njap=0, nlss=0, K=(m>=10)? m: 10, updjac, updp=1, newjac;
|
||||
const int nm=n*m;
|
||||
int (*linsolver)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)=NULL;
|
||||
|
||||
mu=jacTe_inf=p_L2=0.0; /* -Wall */
|
||||
updjac=newjac=0; /* -Wall */
|
||||
|
||||
if(n<m){
|
||||
fprintf(stderr, LCAT(LEVMAR_DIF, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n"), n, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(opts){
|
||||
tau=opts[0];
|
||||
eps1=opts[1];
|
||||
eps2=opts[2];
|
||||
eps2_sq=opts[2]*opts[2];
|
||||
eps3=opts[3];
|
||||
delta=opts[4];
|
||||
if(delta<0.0){
|
||||
delta=-delta; /* make positive */
|
||||
using_ffdif=0; /* use central differencing */
|
||||
}
|
||||
}
|
||||
else{ // use default values
|
||||
tau=LM_CNST(LM_INIT_MU);
|
||||
eps1=LM_CNST(LM_STOP_THRESH);
|
||||
eps2=LM_CNST(LM_STOP_THRESH);
|
||||
eps2_sq=LM_CNST(LM_STOP_THRESH)*LM_CNST(LM_STOP_THRESH);
|
||||
eps3=LM_CNST(LM_STOP_THRESH);
|
||||
delta=LM_CNST(LM_DIFF_DELTA);
|
||||
}
|
||||
|
||||
if(!work){
|
||||
worksz=LM_DIF_WORKSZ(m, n); //4*n+4*m + n*m + m*m;
|
||||
work=(LM_REAL *)malloc(worksz*sizeof(LM_REAL)); /* allocate a big chunk in one step */
|
||||
if(!work){
|
||||
fprintf(stderr, LCAT(LEVMAR_DIF, "(): memory allocation request failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
freework=1;
|
||||
}
|
||||
|
||||
/* set up work arrays */
|
||||
e=work;
|
||||
hx=e + n;
|
||||
jacTe=hx + n;
|
||||
jac=jacTe + m;
|
||||
jacTjac=jac + nm;
|
||||
Dp=jacTjac + m*m;
|
||||
diag_jacTjac=Dp + m;
|
||||
pDp=diag_jacTjac + m;
|
||||
wrk=pDp + m;
|
||||
wrk2=wrk + n;
|
||||
|
||||
/* compute e=x - f(p) and its L2 norm */
|
||||
(*func)(p, hx, m, n, adata); nfev=1;
|
||||
/* ### e=x-hx, p_eL2=||e|| */
|
||||
#if 1
|
||||
p_eL2=LEVMAR_L2NRMXMY(e, x, hx, n);
|
||||
#else
|
||||
for(i=0, p_eL2=0.0; i<n; ++i){
|
||||
e[i]=tmp=x[i]-hx[i];
|
||||
p_eL2+=tmp*tmp;
|
||||
}
|
||||
#endif
|
||||
init_p_eL2=p_eL2;
|
||||
if(!LM_FINITE(p_eL2)) stop=7;
|
||||
|
||||
nu=20; /* force computation of J */
|
||||
|
||||
for(k=0; k<itmax && !stop; ++k){
|
||||
/* Note that p and e have been updated at a previous iteration */
|
||||
|
||||
if(p_eL2<=eps3){ /* error is small */
|
||||
stop=6;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Compute the Jacobian J at p, J^T J, J^T e, ||J^T e||_inf and ||p||^2.
|
||||
* The symmetry of J^T J is again exploited for speed
|
||||
*/
|
||||
|
||||
if((updp && nu>16) || updjac==K){ /* compute difference approximation to J */
|
||||
if(using_ffdif){ /* use forward differences */
|
||||
LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, delta, jac, m, n, adata);
|
||||
++njap; nfev+=m;
|
||||
}
|
||||
else{ /* use central differences */
|
||||
LEVMAR_FDIF_CENT_JAC_APPROX(func, p, wrk, wrk2, delta, jac, m, n, adata);
|
||||
++njap; nfev+=2*m;
|
||||
}
|
||||
nu=2; updjac=0; updp=0; newjac=1;
|
||||
}
|
||||
|
||||
if(newjac){ /* Jacobian has changed, recompute J^T J, J^t e, etc */
|
||||
newjac=0;
|
||||
|
||||
/* J^T J, J^T e */
|
||||
if(nm<=__BLOCKSZ__SQ){ // this is a small problem
|
||||
/* J^T*J_ij = \sum_l J^T_il * J_lj = \sum_l J_li * J_lj.
|
||||
* Thus, the product J^T J can be computed using an outer loop for
|
||||
* l that adds J_li*J_lj to each element ij of the result. Note that
|
||||
* with this scheme, the accesses to J and JtJ are always along rows,
|
||||
* therefore induces less cache misses compared to the straightforward
|
||||
* algorithm for computing the product (i.e., l loop is innermost one).
|
||||
* A similar scheme applies to the computation of J^T e.
|
||||
* However, for large minimization problems (i.e., involving a large number
|
||||
* of unknowns and measurements) for which J/J^T J rows are too large to
|
||||
* fit in the L1 cache, even this scheme incures many cache misses. In
|
||||
* such cases, a cache-efficient blocking scheme is preferable.
|
||||
*
|
||||
* Thanks to John Nitao of Lawrence Livermore Lab for pointing out this
|
||||
* performance problem.
|
||||
*
|
||||
* Note that the non-blocking algorithm is faster on small
|
||||
* problems since in this case it avoids the overheads of blocking.
|
||||
*/
|
||||
register int l;
|
||||
register LM_REAL alpha, *jaclm, *jacTjacim;
|
||||
|
||||
/* looping downwards saves a few computations */
|
||||
for(i=m*m; i-->0; )
|
||||
jacTjac[i]=0.0;
|
||||
for(i=m; i-->0; )
|
||||
jacTe[i]=0.0;
|
||||
|
||||
for(l=n; l-->0; ){
|
||||
jaclm=jac+l*m;
|
||||
for(i=m; i-->0; ){
|
||||
jacTjacim=jacTjac+i*m;
|
||||
alpha=jaclm[i]; //jac[l*m+i];
|
||||
for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
|
||||
jacTjacim[j]+=jaclm[j]*alpha; //jacTjac[i*m+j]+=jac[l*m+j]*alpha
|
||||
|
||||
/* J^T e */
|
||||
jacTe[i]+=alpha*e[l];
|
||||
}
|
||||
}
|
||||
|
||||
for(i=m; i-->0; ) /* copy to upper part */
|
||||
for(j=i+1; j<m; ++j)
|
||||
jacTjac[i*m+j]=jacTjac[j*m+i];
|
||||
}
|
||||
else{ // this is a large problem
|
||||
/* Cache efficient computation of J^T J based on blocking
|
||||
*/
|
||||
LEVMAR_TRANS_MAT_MAT_MULT(jac, jacTjac, n, m);
|
||||
|
||||
/* cache efficient computation of J^T e */
|
||||
for(i=0; i<m; ++i)
|
||||
jacTe[i]=0.0;
|
||||
|
||||
for(i=0; i<n; ++i){
|
||||
register LM_REAL *jacrow;
|
||||
|
||||
for(l=0, jacrow=jac+i*m, tmp=e[i]; l<m; ++l)
|
||||
jacTe[l]+=jacrow[l]*tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/* Compute ||J^T e||_inf and ||p||^2 */
|
||||
for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
|
||||
if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;
|
||||
|
||||
diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
|
||||
p_L2+=p[i]*p[i];
|
||||
}
|
||||
//p_L2=sqrt(p_L2);
|
||||
}
|
||||
|
||||
#if 0
|
||||
if(!(k%100)){
|
||||
printf("Current estimate: ");
|
||||
for(i=0; i<m; ++i)
|
||||
printf("%.9g ", p[i]);
|
||||
printf("-- errors %.9g %0.9g\n", jacTe_inf, p_eL2);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* check for convergence */
|
||||
if((jacTe_inf <= eps1)){
|
||||
Dp_L2=0.0; /* no increment for p in this case */
|
||||
stop=1;
|
||||
break;
|
||||
}
|
||||
|
||||
/* compute initial damping factor */
|
||||
if(k==0){
|
||||
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
|
||||
if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
|
||||
mu=tau*tmp;
|
||||
}
|
||||
|
||||
/* determine increment using adaptive damping */
|
||||
|
||||
/* augment normal equations */
|
||||
for(i=0; i<m; ++i)
|
||||
jacTjac[i*m+i]+=mu;
|
||||
|
||||
/* solve augmented equations */
|
||||
#ifdef HAVE_LAPACK
|
||||
/* 7 alternatives are available: LU, Cholesky + Cholesky with PLASMA, LDLt, 2 variants of QR decomposition and SVD.
|
||||
* For matrices with dimensions of at least a few hundreds, the PLASMA implementation of Cholesky is the fastest.
|
||||
* From the serial solvers, Cholesky is the fastest but might occasionally be inapplicable due to numerical round-off;
|
||||
* QR is slower but more robust; SVD is the slowest but most robust; LU is quite robust but
|
||||
* slower than LDLt; LDLt offers a good tradeoff between robustness and speed
|
||||
*/
|
||||
|
||||
issolved=AX_EQ_B_BK(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_BK;
|
||||
//issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
|
||||
//issolved=AX_EQ_B_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_CHOL;
|
||||
#ifdef HAVE_PLASMA
|
||||
//issolved=AX_EQ_B_PLASMA_CHOL(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_PLASMA_CHOL;
|
||||
#endif
|
||||
//issolved=AX_EQ_B_QR(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_QR;
|
||||
//issolved=AX_EQ_B_QRLS(jacTjac, jacTe, Dp, m, m); ++nlss; linsolver=(int (*)(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m))AX_EQ_B_QRLS;
|
||||
//issolved=AX_EQ_B_SVD(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_SVD;
|
||||
#else
|
||||
/* use the LU included with levmar */
|
||||
issolved=AX_EQ_B_LU(jacTjac, jacTe, Dp, m); ++nlss; linsolver=AX_EQ_B_LU;
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
if(issolved){
|
||||
/* compute p's new estimate and ||Dp||^2 */
|
||||
for(i=0, Dp_L2=0.0; i<m; ++i){
|
||||
pDp[i]=p[i] + (tmp=Dp[i]);
|
||||
Dp_L2+=tmp*tmp;
|
||||
}
|
||||
//Dp_L2=sqrt(Dp_L2);
|
||||
|
||||
if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
|
||||
//if(Dp_L2<=eps2*(p_L2 + eps2)){ /* relative change in p is small, stop */
|
||||
stop=2;
|
||||
break;
|
||||
}
|
||||
|
||||
if(Dp_L2>=(p_L2+eps2)/(LM_CNST(EPSILON)*LM_CNST(EPSILON))){ /* almost singular */
|
||||
//if(Dp_L2>=(p_L2+eps2)/LM_CNST(EPSILON)){ /* almost singular */
|
||||
stop=4;
|
||||
break;
|
||||
}
|
||||
|
||||
(*func)(pDp, wrk, m, n, adata); ++nfev; /* evaluate function at p + Dp */
|
||||
/* compute ||e(pDp)||_2 */
|
||||
/* ### wrk2=x-wrk, pDp_eL2=||wrk2|| */
|
||||
#if 1
|
||||
pDp_eL2=LEVMAR_L2NRMXMY(wrk2, x, wrk, n);
|
||||
#else
|
||||
for(i=0, pDp_eL2=0.0; i<n; ++i){
|
||||
wrk2[i]=tmp=x[i]-wrk[i];
|
||||
pDp_eL2+=tmp*tmp;
|
||||
}
|
||||
#endif
|
||||
if(!LM_FINITE(pDp_eL2)){ /* sum of squares is not finite, most probably due to a user error.
|
||||
* This check makes sure that the loop terminates early in the case
|
||||
* of invalid input. Thanks to Steve Danauskas for suggesting it
|
||||
*/
|
||||
|
||||
stop=7;
|
||||
break;
|
||||
}
|
||||
|
||||
dF=p_eL2-pDp_eL2;
|
||||
if(updp || dF>0){ /* update jac */
|
||||
for(i=0; i<n; ++i){
|
||||
for(l=0, tmp=0.0; l<m; ++l)
|
||||
tmp+=jac[i*m+l]*Dp[l]; /* (J * Dp)[i] */
|
||||
tmp=(wrk[i] - hx[i] - tmp)/Dp_L2; /* (f(p+dp)[i] - f(p)[i] - (J * Dp)[i])/(dp^T*dp) */
|
||||
for(j=0; j<m; ++j)
|
||||
jac[i*m+j]+=tmp*Dp[j];
|
||||
}
|
||||
++updjac;
|
||||
newjac=1;
|
||||
}
|
||||
|
||||
for(i=0, dL=0.0; i<m; ++i)
|
||||
dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);
|
||||
|
||||
if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
|
||||
tmp=(LM_CNST(2.0)*dF/dL-LM_CNST(1.0));
|
||||
tmp=LM_CNST(1.0)-tmp*tmp*tmp;
|
||||
mu=mu*( (tmp>=LM_CNST(ONE_THIRD))? tmp : LM_CNST(ONE_THIRD) );
|
||||
nu=2;
|
||||
|
||||
for(i=0 ; i<m; ++i) /* update p's estimate */
|
||||
p[i]=pDp[i];
|
||||
|
||||
for(i=0; i<n; ++i){ /* update e, hx and ||e||_2 */
|
||||
e[i]=wrk2[i]; //x[i]-wrk[i];
|
||||
hx[i]=wrk[i];
|
||||
}
|
||||
p_eL2=pDp_eL2;
|
||||
updp=1;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
/* if this point is reached, either the linear system could not be solved or
|
||||
* the error did not reduce; in any case, the increment must be rejected
|
||||
*/
|
||||
|
||||
mu*=nu;
|
||||
nu2=nu<<1; // 2*nu;
|
||||
if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
|
||||
stop=5;
|
||||
break;
|
||||
}
|
||||
nu=nu2;
|
||||
|
||||
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
|
||||
jacTjac[i*m+i]=diag_jacTjac[i];
|
||||
}
|
||||
|
||||
if(k>=itmax) stop=3;
|
||||
|
||||
for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
|
||||
jacTjac[i*m+i]=diag_jacTjac[i];
|
||||
|
||||
if(info){
|
||||
info[0]=init_p_eL2;
|
||||
info[1]=p_eL2;
|
||||
info[2]=jacTe_inf;
|
||||
info[3]=Dp_L2;
|
||||
for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
|
||||
if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
|
||||
info[4]=mu/tmp;
|
||||
info[5]=(LM_REAL)k;
|
||||
info[6]=(LM_REAL)stop;
|
||||
info[7]=(LM_REAL)nfev;
|
||||
info[8]=(LM_REAL)njap;
|
||||
info[9]=(LM_REAL)nlss;
|
||||
}
|
||||
|
||||
/* covariance matrix */
|
||||
if(covar){
|
||||
LEVMAR_COVAR(jacTjac, covar, p_eL2, m, n);
|
||||
}
|
||||
|
||||
|
||||
if(freework) free(work);
|
||||
|
||||
#ifdef LINSOLVERS_RETAIN_MEMORY
|
||||
if(linsolver) (*linsolver)(NULL, NULL, NULL, 0);
|
||||
#endif
|
||||
|
||||
return (stop!=4 && stop!=7)? k : LM_ERROR;
|
||||
}
|
||||
|
||||
/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
|
||||
#undef LEVMAR_DER
|
||||
#undef LEVMAR_DIF
|
||||
#undef LEVMAR_FDIF_FORW_JAC_APPROX
|
||||
#undef LEVMAR_FDIF_CENT_JAC_APPROX
|
||||
#undef LEVMAR_COVAR
|
||||
#undef LEVMAR_TRANS_MAT_MAT_MULT
|
||||
#undef LEVMAR_L2NRMXMY
|
||||
#undef AX_EQ_B_LU
|
||||
#undef AX_EQ_B_CHOL
|
||||
#undef AX_EQ_B_PLASMA_CHOL
|
||||
#undef AX_EQ_B_QR
|
||||
#undef AX_EQ_B_QRLS
|
||||
#undef AX_EQ_B_SVD
|
||||
#undef AX_EQ_B_BK
|
||||
@@ -1,87 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/********************************************************************************
|
||||
* Box-constrained Levenberg-Marquardt nonlinear minimization. The same core code
|
||||
* is used with appropriate #defines to derive single and double precision versions,
|
||||
* see also lmbc_core.c
|
||||
********************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "compiler.h"
|
||||
#include "misc.h"
|
||||
|
||||
#define EPSILON 1E-12
|
||||
#define ONE_THIRD 0.3333333334 /* 1.0/3.0 */
|
||||
#define _LSITMAX_ 150 /* max #iterations for line search */
|
||||
#define _POW_ 2.1
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define LM_REAL_MAX FLT_MAX
|
||||
#define LM_REAL_MIN -FLT_MAX
|
||||
|
||||
#define LM_REAL_EPSILON FLT_EPSILON
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "lmbc_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_REAL_MAX DBL_MAX
|
||||
#define LM_REAL_MIN -DBL_MAX
|
||||
|
||||
#define LM_REAL_EPSILON DBL_EPSILON
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "lmbc_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,87 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/********************************************************************************
|
||||
* combined box and linear equation constraints Levenberg-Marquardt nonlinear
|
||||
* minimization. The same core code is used with appropriate #defines to derive
|
||||
* single and double precision versions, see also lmblec_core.c
|
||||
********************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "misc.h"
|
||||
|
||||
#ifndef HAVE_LAPACK
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("Combined box and linearly constrained optimization requires LAPACK and was not compiled!")
|
||||
#else
|
||||
#warning Combined box and linearly constrained optimization requires LAPACK and was not compiled!
|
||||
#endif // _MSC_VER
|
||||
|
||||
#else // LAPACK present
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define LM_REAL_MAX FLT_MAX
|
||||
#define LM_REAL_MIN -FLT_MAX
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "lmblec_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_REAL_MAX DBL_MAX
|
||||
#define LM_REAL_MIN -DBL_MAX
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "lmblec_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#endif /* HAVE_LAPACK */
|
||||
@@ -1,413 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-06 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*******************************************************************************
|
||||
* This file implements combined box and linear equation constraints.
|
||||
*
|
||||
* Note that the algorithm implementing linearly constrained minimization does
|
||||
* so by a change in parameters that transforms the original program into an
|
||||
* unconstrained one. To employ the same idea for implementing box & linear
|
||||
* constraints would require the transformation of box constraints on the
|
||||
* original parameters to box constraints for the new parameter set. This
|
||||
* being impossible, a different approach is used here for finding the minimum.
|
||||
* The trick is to remove the box constraints by augmenting the function to
|
||||
* be fitted with penalty terms and then solve the resulting problem (which
|
||||
* involves linear constrains only) with the functions in lmlec.c
|
||||
*
|
||||
* More specifically, for the constraint a<=x[i]<=b to hold, the term C[i]=
|
||||
* (2*x[i]-(a+b))/(b-a) should be within [-1, 1]. This is enforced by adding
|
||||
* the penalty term w[i]*max((C[i])^2-1, 0) to the objective function, where
|
||||
* w[i] is a large weight. In the case of constraints of the form a<=x[i],
|
||||
* the term C[i]=a-x[i] has to be non positive, thus the penalty term is
|
||||
* w[i]*max(C[i], 0). If x[i]<=b, C[i]=x[i]-b has to be non negative and
|
||||
* the penalty is w[i]*max(C[i], 0). The derivatives needed for the Jacobian
|
||||
* are as follows:
|
||||
* For the constraint a<=x[i]<=b: 4*(2*x[i]-(a+b))/(b-a)^2 if x[i] not in [a, b],
|
||||
* 0 otherwise
|
||||
* For the constraint a<=x[i]: -1 if x[i]<=a, 0 otherwise
|
||||
* For the constraint x[i]<=b: 1 if b<=x[i], 0 otherwise
|
||||
*
|
||||
* Note that for the above to work, the weights w[i] should be large enough;
|
||||
* depending on your minimization problem, the default values might need some
|
||||
* tweaking (see arg "wghts" below).
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef LM_REAL // not included by lmblec.c
|
||||
#error This file should not be compiled directly!
|
||||
#endif
|
||||
|
||||
|
||||
#define __MAX__(x, y) (((x)>=(y))? (x) : (y))
|
||||
#define __BC_WEIGHT__ LM_CNST(1E+04)
|
||||
|
||||
#define __BC_INTERVAL__ 0
|
||||
#define __BC_LOW__ 1
|
||||
#define __BC_HIGH__ 2
|
||||
|
||||
/* precision-specific definitions */
|
||||
#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
|
||||
#define LMBLEC_DATA LM_ADD_PREFIX(lmblec_data)
|
||||
#define LMBLEC_FUNC LM_ADD_PREFIX(lmblec_func)
|
||||
#define LMBLEC_JACF LM_ADD_PREFIX(lmblec_jacf)
|
||||
#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
|
||||
#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
|
||||
#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)
|
||||
#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)
|
||||
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
||||
|
||||
struct LMBLEC_DATA{
|
||||
LM_REAL *x, *lb, *ub, *w;
|
||||
int *bctype;
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
|
||||
void *adata;
|
||||
};
|
||||
|
||||
/* augmented measurements */
|
||||
static void LMBLEC_FUNC(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata)
|
||||
{
|
||||
struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
|
||||
int nn;
|
||||
register int i, j, *typ;
|
||||
register LM_REAL *lb, *ub, *w, tmp;
|
||||
|
||||
nn=n-m;
|
||||
lb=data->lb;
|
||||
ub=data->ub;
|
||||
w=data->w;
|
||||
typ=data->bctype;
|
||||
(*(data->func))(p, hx, m, nn, data->adata);
|
||||
|
||||
for(i=nn, j=0; i<n; ++i, ++j){
|
||||
switch(typ[j]){
|
||||
case __BC_INTERVAL__:
|
||||
tmp=(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(ub[j]-lb[j]);
|
||||
hx[i]=w[j]*__MAX__(tmp*tmp-LM_CNST(1.0), LM_CNST(0.0));
|
||||
break;
|
||||
|
||||
case __BC_LOW__:
|
||||
hx[i]=w[j]*__MAX__(lb[j]-p[j], LM_CNST(0.0));
|
||||
break;
|
||||
|
||||
case __BC_HIGH__:
|
||||
hx[i]=w[j]*__MAX__(p[j]-ub[j], LM_CNST(0.0));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* augmented Jacobian */
|
||||
static void LMBLEC_JACF(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata)
|
||||
{
|
||||
struct LMBLEC_DATA *data=(struct LMBLEC_DATA *)adata;
|
||||
int nn, *typ;
|
||||
register int i, j;
|
||||
register LM_REAL *lb, *ub, *w, tmp;
|
||||
|
||||
nn=n-m;
|
||||
lb=data->lb;
|
||||
ub=data->ub;
|
||||
w=data->w;
|
||||
typ=data->bctype;
|
||||
(*(data->jacf))(p, jac, m, nn, data->adata);
|
||||
|
||||
/* clear all extra rows */
|
||||
for(i=nn*m; i<n*m; ++i)
|
||||
jac[i]=0.0;
|
||||
|
||||
for(i=nn, j=0; i<n; ++i, ++j){
|
||||
switch(typ[j]){
|
||||
case __BC_INTERVAL__:
|
||||
if(lb[j]<=p[j] && p[j]<=ub[j])
|
||||
continue; // corresp. jac element already 0
|
||||
|
||||
/* out of interval */
|
||||
tmp=ub[j]-lb[j];
|
||||
tmp=LM_CNST(4.0)*(LM_CNST(2.0)*p[j]-(lb[j]+ub[j]))/(tmp*tmp);
|
||||
jac[i*m+j]=w[j]*tmp;
|
||||
break;
|
||||
|
||||
case __BC_LOW__: // (lb[j]<=p[j])? 0.0 : -1.0;
|
||||
if(lb[j]<=p[j])
|
||||
continue; // corresp. jac element already 0
|
||||
|
||||
/* smaller than lower bound */
|
||||
jac[i*m+j]=-w[j];
|
||||
break;
|
||||
|
||||
case __BC_HIGH__: // (p[j]<=ub[j])? 0.0 : 1.0;
|
||||
if(p[j]<=ub[j])
|
||||
continue; // corresp. jac element already 0
|
||||
|
||||
/* greater than upper bound */
|
||||
jac[i*m+j]=w[j];
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* This function seeks the parameter vector p that best describes the measurements
|
||||
* vector x under box & linear constraints.
|
||||
* More precisely, given a vector function func : R^m --> R^n with n>=m,
|
||||
* it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
|
||||
* e=x-func(p) is minimized under the constraints lb[i]<=p[i]<=ub[i] and A p=b;
|
||||
* A is kxm, b kx1. Note that this function DOES NOT check the satisfiability of
|
||||
* the specified box and linear equation constraints.
|
||||
* If no lower bound constraint applies for p[i], use -DBL_MAX/-FLT_MAX for lb[i];
|
||||
* If no upper bound constraint applies for p[i], use DBL_MAX/FLT_MAX for ub[i].
|
||||
*
|
||||
* This function requires an analytic Jacobian. In case the latter is unavailable,
|
||||
* use LEVMAR_BLEC_DIF() bellow
|
||||
*
|
||||
* Returns the number of iterations (>=0) if successful, LM_ERROR if failed
|
||||
*
|
||||
* For more details on the algorithm implemented by this function, please refer to
|
||||
* the comments in the top of this file.
|
||||
*
|
||||
*/
|
||||
int LEVMAR_BLEC_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
|
||||
LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
|
||||
LM_REAL *A, /* I: constraints matrix, kxm */
|
||||
LM_REAL *b, /* I: right hand constraints vector, kx1 */
|
||||
int k, /* I: number of constraints (i.e. A's #rows) */
|
||||
LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */
|
||||
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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated 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 LMBLEC_DATA data;
|
||||
int ret;
|
||||
LM_REAL locinfo[LM_INFO_SZ];
|
||||
register int i;
|
||||
|
||||
if(!jacf){
|
||||
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEC_DER)
|
||||
RCAT("().\nIf no such function is available, use ", LEVMAR_BLEC_DIF) RCAT("() rather than ", LEVMAR_BLEC_DER) "()\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!lb && !ub){
|
||||
fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DER, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
|
||||
LEVMAR_LEC_DER) "() in this case!\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!LEVMAR_BOX_CHECK(lb, ub, m)){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* measurement vector needs to be extended by m */
|
||||
if(x){ /* nonzero x */
|
||||
data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
|
||||
if(!data.x){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
for(i=0; i<n; ++i)
|
||||
data.x[i]=x[i];
|
||||
for(i=n; i<n+m; ++i)
|
||||
data.x[i]=0.0;
|
||||
}
|
||||
else
|
||||
data.x=NULL;
|
||||
|
||||
data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
|
||||
if(!data.w){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
|
||||
if(data.x) free(data.x);
|
||||
return LM_ERROR;
|
||||
}
|
||||
data.bctype=(int *)(data.w+m);
|
||||
|
||||
/* note: at this point, one of lb, ub are not NULL */
|
||||
for(i=0; i<m; ++i){
|
||||
data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
|
||||
if(!lb) data.bctype[i]=__BC_HIGH__;
|
||||
else if(!ub) data.bctype[i]=__BC_LOW__;
|
||||
else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
|
||||
else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
|
||||
else data.bctype[i]=__BC_HIGH__;
|
||||
}
|
||||
|
||||
data.lb=lb;
|
||||
data.ub=ub;
|
||||
data.func=func;
|
||||
data.jacf=jacf;
|
||||
data.adata=adata;
|
||||
|
||||
if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DER() is called with non-null info */
|
||||
ret=LEVMAR_LEC_DER(LMBLEC_FUNC, LMBLEC_JACF, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
|
||||
|
||||
if(data.x) free(data.x);
|
||||
free(data.w);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Similar to the LEVMAR_BLEC_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_BLEC_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
|
||||
LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
|
||||
LM_REAL *A, /* I: constraints matrix, kxm */
|
||||
LM_REAL *b, /* I: right hand constraints vector, kx1 */
|
||||
int k, /* I: number of constraints (i.e. A's #rows) */
|
||||
LM_REAL *wghts, /* mx1 weights for penalty terms, defaults used if NULL */
|
||||
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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_BLEC_DIF_WORKSZ() reals large, allocated 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 LMBLEC_DATA data;
|
||||
int ret;
|
||||
register int i;
|
||||
LM_REAL locinfo[LM_INFO_SZ];
|
||||
|
||||
if(!lb && !ub){
|
||||
fprintf(stderr, RCAT(LCAT(LEVMAR_BLEC_DIF, "(): lower and upper bounds for box constraints cannot be both NULL, use "),
|
||||
LEVMAR_LEC_DIF) "() in this case!\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!LEVMAR_BOX_CHECK(lb, ub, m)){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): at least one lower bound exceeds the upper one\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* measurement vector needs to be extended by m */
|
||||
if(x){ /* nonzero x */
|
||||
data.x=(LM_REAL *)malloc((n+m)*sizeof(LM_REAL));
|
||||
if(!data.x){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #1 failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
for(i=0; i<n; ++i)
|
||||
data.x[i]=x[i];
|
||||
for(i=n; i<n+m; ++i)
|
||||
data.x[i]=0.0;
|
||||
}
|
||||
else
|
||||
data.x=NULL;
|
||||
|
||||
data.w=(LM_REAL *)malloc(m*sizeof(LM_REAL) + m*sizeof(int)); /* should be arranged in that order for proper doubles alignment */
|
||||
if(!data.w){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEC_DER, "(): memory allocation request #2 failed\n"));
|
||||
if(data.x) free(data.x);
|
||||
return LM_ERROR;
|
||||
}
|
||||
data.bctype=(int *)(data.w+m);
|
||||
|
||||
/* note: at this point, one of lb, ub are not NULL */
|
||||
for(i=0; i<m; ++i){
|
||||
data.w[i]=(!wghts)? __BC_WEIGHT__ : wghts[i];
|
||||
if(!lb) data.bctype[i]=__BC_HIGH__;
|
||||
else if(!ub) data.bctype[i]=__BC_LOW__;
|
||||
else if(ub[i]!=LM_REAL_MAX && lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_INTERVAL__;
|
||||
else if(lb[i]!=LM_REAL_MIN) data.bctype[i]=__BC_LOW__;
|
||||
else data.bctype[i]=__BC_HIGH__;
|
||||
}
|
||||
|
||||
data.lb=lb;
|
||||
data.ub=ub;
|
||||
data.func=func;
|
||||
data.jacf=NULL;
|
||||
data.adata=adata;
|
||||
|
||||
if(!info) info=locinfo; /* make sure that LEVMAR_LEC_DIF() is called with non-null info */
|
||||
ret=LEVMAR_LEC_DIF(LMBLEC_FUNC, p, data.x, m, n+m, A, b, k, itmax, opts, info, work, covar, (void *)&data);
|
||||
|
||||
if(data.x) free(data.x);
|
||||
free(data.w);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
|
||||
#undef LEVMAR_BOX_CHECK
|
||||
#undef LMBLEC_DATA
|
||||
#undef LMBLEC_FUNC
|
||||
#undef LMBLEC_JACF
|
||||
#undef LEVMAR_COVAR
|
||||
#undef LEVMAR_LEC_DER
|
||||
#undef LEVMAR_LEC_DIF
|
||||
#undef LEVMAR_BLEC_DER
|
||||
#undef LEVMAR_BLEC_DIF
|
||||
@@ -1,89 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2009 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*******************************************************************************
|
||||
* Wrappers for linear inequality constrained Levenberg-Marquardt minimization.
|
||||
* The same core code is used with appropriate #defines to derive single and
|
||||
* double precision versions, see also lmbleic_core.c
|
||||
*******************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "misc.h"
|
||||
|
||||
|
||||
#ifndef HAVE_LAPACK
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("Linear inequalities constrained optimization requires LAPACK and was not compiled!")
|
||||
#else
|
||||
#warning Linear inequalities constrained optimization requires LAPACK and was not compiled!
|
||||
#endif // _MSC_VER
|
||||
|
||||
#else // LAPACK present
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define LM_REAL_MAX FLT_MAX
|
||||
#define LM_REAL_MIN -FLT_MAX
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "lmbleic_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_REAL_MAX DBL_MAX
|
||||
#define LM_REAL_MIN -DBL_MAX
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "lmbleic_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_MAX
|
||||
#undef LM_REAL_MIN
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
@@ -1,506 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2009 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef LM_REAL // not included by lmbleic.c
|
||||
#error This file should not be compiled directly!
|
||||
#endif
|
||||
|
||||
|
||||
/* precision-specific definitions */
|
||||
#define LMBLEIC_DATA LM_ADD_PREFIX(lmbleic_data)
|
||||
#define LMBLEIC_ELIM LM_ADD_PREFIX(lmbleic_elim)
|
||||
#define LMBLEIC_FUNC LM_ADD_PREFIX(lmbleic_func)
|
||||
#define LMBLEIC_JACF LM_ADD_PREFIX(lmbleic_jacf)
|
||||
#define LEVMAR_BLEIC_DER LM_ADD_PREFIX(levmar_bleic_der)
|
||||
#define LEVMAR_BLEIC_DIF LM_ADD_PREFIX(levmar_bleic_dif)
|
||||
#define LEVMAR_BLIC_DER LM_ADD_PREFIX(levmar_blic_der)
|
||||
#define LEVMAR_BLIC_DIF LM_ADD_PREFIX(levmar_blic_dif)
|
||||
#define LEVMAR_LEIC_DER LM_ADD_PREFIX(levmar_leic_der)
|
||||
#define LEVMAR_LEIC_DIF LM_ADD_PREFIX(levmar_leic_dif)
|
||||
#define LEVMAR_LIC_DER LM_ADD_PREFIX(levmar_lic_der)
|
||||
#define LEVMAR_LIC_DIF LM_ADD_PREFIX(levmar_lic_dif)
|
||||
#define LEVMAR_BLEC_DER LM_ADD_PREFIX(levmar_blec_der)
|
||||
#define LEVMAR_BLEC_DIF LM_ADD_PREFIX(levmar_blec_dif)
|
||||
#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
|
||||
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
||||
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
|
||||
|
||||
struct LMBLEIC_DATA{
|
||||
LM_REAL *jac;
|
||||
int nineqcnstr; // #inequality constraints
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
|
||||
void *adata;
|
||||
};
|
||||
|
||||
|
||||
/* wrapper ensuring that the user-supplied function is called with the right number of variables (i.e. m) */
|
||||
static void LMBLEIC_FUNC(LM_REAL *pext, LM_REAL *hx, int mm, int n, void *adata)
|
||||
{
|
||||
struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata;
|
||||
int m;
|
||||
|
||||
m=mm-data->nineqcnstr;
|
||||
(*(data->func))(pext, hx, m, n, data->adata);
|
||||
}
|
||||
|
||||
|
||||
/* wrapper for computing the Jacobian at pext. The Jacobian is nxmm */
|
||||
static void LMBLEIC_JACF(LM_REAL *pext, LM_REAL *jacext, int mm, int n, void *adata)
|
||||
{
|
||||
struct LMBLEIC_DATA *data=(struct LMBLEIC_DATA *)adata;
|
||||
int m;
|
||||
register int i, j;
|
||||
LM_REAL *jac, *jacim, *jacextimm;
|
||||
|
||||
m=mm-data->nineqcnstr;
|
||||
jac=data->jac;
|
||||
|
||||
(*(data->jacf))(pext, jac, m, n, data->adata);
|
||||
|
||||
for(i=0; i<n; ++i){
|
||||
jacextimm=jacext+i*mm;
|
||||
jacim=jac+i*m;
|
||||
for(j=0; j<m; ++j)
|
||||
jacextimm[j]=jacim[j]; //jacext[i*mm+j]=jac[i*m+j];
|
||||
|
||||
for(j=m; j<mm; ++j)
|
||||
jacextimm[j]=0.0; //jacext[i*mm+j]=0.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* This function is similar to LEVMAR_DER except that the minimization is
|
||||
* performed subject to the box constraints lb[i]<=p[i]<=ub[i], the linear
|
||||
* equation constraints A*p=b, A being k1xm, b k1x1, and the linear inequality
|
||||
* constraints C*p>=d, C being k2xm, d k2x1.
|
||||
*
|
||||
* The inequalities are converted to equations by introducing surplus variables,
|
||||
* i.e. c^T*p >= d becomes c^T*p - y = d, with y>=0. To transform all inequalities
|
||||
* to equations, a total of k2 surplus variables are introduced; a problem with only
|
||||
* box and linear constraints results then and is solved with LEVMAR_BLEC_DER()
|
||||
* Note that opposite direction inequalities should be converted to the desired
|
||||
* direction by negating, i.e. c^T*p <= d becomes -c^T*p >= -d
|
||||
*
|
||||
* This function requires an analytic Jacobian. In case the latter is unavailable,
|
||||
* use LEVMAR_BLEIC_DIF() bellow
|
||||
*
|
||||
*/
|
||||
int LEVMAR_BLEIC_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
|
||||
LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
|
||||
LM_REAL *A, /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */
|
||||
LM_REAL *b, /* I: right hand constraints vector, k1x1 */
|
||||
int k1, /* I: number of constraints (i.e. A's #rows) */
|
||||
LM_REAL *C, /* I: inequality constraints matrix, k2xm */
|
||||
LM_REAL *d, /* I: right hand constraints vector, k2x1 */
|
||||
int k2, /* I: number of inequality constraints (i.e. C'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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_BLEIC_DER_WORKSZ() reals large, allocated 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 LMBLEIC_DATA data;
|
||||
LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables;
|
||||
pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm
|
||||
*/
|
||||
LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables
|
||||
int mm, ret, k12;
|
||||
register int i, j, ii;
|
||||
register LM_REAL tmp;
|
||||
LM_REAL locinfo[LM_INFO_SZ];
|
||||
|
||||
if(!jacf){
|
||||
fprintf(stderr, RCAT("No function specified for computing the Jacobian in ", LEVMAR_BLEIC_DER)
|
||||
RCAT("().\nIf no such function is available, use ", LEVMAR_BLEIC_DIF) RCAT("() rather than ", LEVMAR_BLEIC_DER) "()\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!C || !d){
|
||||
fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DER, "(): missing inequality constraints, use "), LEVMAR_BLEC_DER) "() in this case!\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
if(!A || !b) k1=0; // sanity check
|
||||
|
||||
mm=m+k2;
|
||||
|
||||
if(n<m-k1){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
k12=k1+k2;
|
||||
ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + n*m + (covar? mm*mm : 0))*sizeof(LM_REAL));
|
||||
if(!ptr){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEIC_DER, "(): memory allocation request failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
pext=ptr;
|
||||
lbext=pext+mm;
|
||||
ubext=lbext+mm;
|
||||
Aext=ubext+mm;
|
||||
bext=Aext+k12*mm;
|
||||
data.jac=bext+k12;
|
||||
covext=covar? data.jac+n*m : NULL;
|
||||
data.nineqcnstr=k2;
|
||||
data.func=func;
|
||||
data.jacf=jacf;
|
||||
data.adata=adata;
|
||||
|
||||
/* compute y s.t. C*p - y=d, i.e. y=C*p-d.
|
||||
* y is stored in the last k2 elements of pext
|
||||
*/
|
||||
for(i=0; i<k2; ++i){
|
||||
for(j=0, tmp=0.0; j<m; ++j)
|
||||
tmp+=C[i*m+j]*p[j];
|
||||
pext[j=i+m]=tmp-d[i];
|
||||
|
||||
/* surplus variables must be >=0 */
|
||||
lbext[j]=0.0;
|
||||
ubext[j]=LM_REAL_MAX;
|
||||
}
|
||||
/* set the first m elements of pext equal to p */
|
||||
for(i=0; i<m; ++i){
|
||||
pext[i]=p[i];
|
||||
lbext[i]=lb? lb[i] : LM_REAL_MIN;
|
||||
ubext[i]=ub? ub[i] : LM_REAL_MAX;
|
||||
}
|
||||
|
||||
/* setup the constraints matrix */
|
||||
/* original linear equation constraints */
|
||||
for(i=0; i<k1; ++i){
|
||||
for(j=0; j<m; ++j)
|
||||
Aext[i*mm+j]=A[i*m+j];
|
||||
|
||||
for(j=m; j<mm; ++j)
|
||||
Aext[i*mm+j]=0.0;
|
||||
|
||||
bext[i]=b[i];
|
||||
}
|
||||
/* linear equation constraints resulting from surplus variables */
|
||||
for(i=0, ii=k1; i<k2; ++i, ++ii){
|
||||
for(j=0; j<m; ++j)
|
||||
Aext[ii*mm+j]=C[i*m+j];
|
||||
|
||||
for(j=m; j<mm; ++j)
|
||||
Aext[ii*mm+j]=0.0;
|
||||
|
||||
Aext[ii*mm+m+i]=-1.0;
|
||||
|
||||
bext[ii]=d[i];
|
||||
}
|
||||
|
||||
if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DER() is called with non-null info */
|
||||
/* note that the default weights for the penalty terms are being used below */
|
||||
ret=LEVMAR_BLEC_DER(LMBLEIC_FUNC, LMBLEIC_JACF, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data);
|
||||
|
||||
/* copy back the minimizer */
|
||||
for(i=0; i<m; ++i)
|
||||
p[i]=pext[i];
|
||||
|
||||
#if 0
|
||||
printf("Surplus variables for the minimizer:\n");
|
||||
for(i=m; i<mm; ++i)
|
||||
printf("%g ", pext[i]);
|
||||
printf("\n\n");
|
||||
#endif
|
||||
|
||||
if(covar){
|
||||
for(i=0; i<m; ++i){
|
||||
for(j=0; j<m; ++j)
|
||||
covar[i*m+j]=covext[i*mm+j];
|
||||
}
|
||||
}
|
||||
|
||||
free(ptr);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Similar to the LEVMAR_BLEIC_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_BLEIC_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. NULL implies a zero vector */
|
||||
int m, /* I: parameter vector dimension (i.e. #unknowns) */
|
||||
int n, /* I: measurement vector dimension */
|
||||
LM_REAL *lb, /* I: vector of lower bounds. If NULL, no lower bounds apply */
|
||||
LM_REAL *ub, /* I: vector of upper bounds. If NULL, no upper bounds apply */
|
||||
LM_REAL *A, /* I: equality constraints matrix, k1xm. If NULL, no linear equation constraints apply */
|
||||
LM_REAL *b, /* I: right hand constraints vector, k1x1 */
|
||||
int k1, /* I: number of constraints (i.e. A's #rows) */
|
||||
LM_REAL *C, /* I: inequality constraints matrix, k2xm */
|
||||
LM_REAL *d, /* I: right hand constraints vector, k2x1 */
|
||||
int k2, /* I: number of inequality constraints (i.e. C'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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_BLEIC_DIF_WORKSZ() reals large, allocated 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 LMBLEIC_DATA data;
|
||||
LM_REAL *ptr, *pext, *Aext, *bext, *covext; /* corresponding to p, A, b, covar for the full set of variables;
|
||||
pext=[p, surplus], pext is mm, Aext is (k1+k2)xmm, bext (k1+k2), covext is mmxmm
|
||||
*/
|
||||
LM_REAL *lbext, *ubext; // corresponding to lb, ub for the full set of variables
|
||||
int mm, ret, k12;
|
||||
register int i, j, ii;
|
||||
register LM_REAL tmp;
|
||||
LM_REAL locinfo[LM_INFO_SZ];
|
||||
|
||||
if(!C || !d){
|
||||
fprintf(stderr, RCAT(LCAT(LEVMAR_BLEIC_DIF, "(): missing inequality constraints, use "), LEVMAR_BLEC_DIF) "() in this case!\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
if(!A || !b) k1=0; // sanity check
|
||||
|
||||
mm=m+k2;
|
||||
|
||||
if(n<m-k1){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k1, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
k12=k1+k2;
|
||||
ptr=(LM_REAL *)malloc((3*mm + k12*mm + k12 + (covar? mm*mm : 0))*sizeof(LM_REAL));
|
||||
if(!ptr){
|
||||
fprintf(stderr, LCAT(LEVMAR_BLEIC_DIF, "(): memory allocation request failed\n"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
pext=ptr;
|
||||
lbext=pext+mm;
|
||||
ubext=lbext+mm;
|
||||
Aext=ubext+mm;
|
||||
bext=Aext+k12*mm;
|
||||
data.jac=NULL;
|
||||
covext=covar? bext+k12 : NULL;
|
||||
data.nineqcnstr=k2;
|
||||
data.func=func;
|
||||
data.jacf=NULL;
|
||||
data.adata=adata;
|
||||
|
||||
/* compute y s.t. C*p - y=d, i.e. y=C*p-d.
|
||||
* y is stored in the last k2 elements of pext
|
||||
*/
|
||||
for(i=0; i<k2; ++i){
|
||||
for(j=0, tmp=0.0; j<m; ++j)
|
||||
tmp+=C[i*m+j]*p[j];
|
||||
pext[j=i+m]=tmp-d[i];
|
||||
|
||||
/* surplus variables must be >=0 */
|
||||
lbext[j]=0.0;
|
||||
ubext[j]=LM_REAL_MAX;
|
||||
}
|
||||
/* set the first m elements of pext equal to p */
|
||||
for(i=0; i<m; ++i){
|
||||
pext[i]=p[i];
|
||||
lbext[i]=lb? lb[i] : LM_REAL_MIN;
|
||||
ubext[i]=ub? ub[i] : LM_REAL_MAX;
|
||||
}
|
||||
|
||||
/* setup the constraints matrix */
|
||||
/* original linear equation constraints */
|
||||
for(i=0; i<k1; ++i){
|
||||
for(j=0; j<m; ++j)
|
||||
Aext[i*mm+j]=A[i*m+j];
|
||||
|
||||
for(j=m; j<mm; ++j)
|
||||
Aext[i*mm+j]=0.0;
|
||||
|
||||
bext[i]=b[i];
|
||||
}
|
||||
/* linear equation constraints resulting from surplus variables */
|
||||
for(i=0, ii=k1; i<k2; ++i, ++ii){
|
||||
for(j=0; j<m; ++j)
|
||||
Aext[ii*mm+j]=C[i*m+j];
|
||||
|
||||
for(j=m; j<mm; ++j)
|
||||
Aext[ii*mm+j]=0.0;
|
||||
|
||||
Aext[ii*mm+m+i]=-1.0;
|
||||
|
||||
bext[ii]=d[i];
|
||||
}
|
||||
|
||||
if(!info) info=locinfo; /* make sure that LEVMAR_BLEC_DIF() is called with non-null info */
|
||||
/* note that the default weights for the penalty terms are being used below */
|
||||
ret=LEVMAR_BLEC_DIF(LMBLEIC_FUNC, pext, x, mm, n, lbext, ubext, Aext, bext, k12, NULL, itmax, opts, info, work, covext, (void *)&data);
|
||||
|
||||
/* copy back the minimizer */
|
||||
for(i=0; i<m; ++i)
|
||||
p[i]=pext[i];
|
||||
|
||||
#if 0
|
||||
printf("Surplus variables for the minimizer:\n");
|
||||
for(i=m; i<mm; ++i)
|
||||
printf("%g ", pext[i]);
|
||||
printf("\n\n");
|
||||
#endif
|
||||
|
||||
if(covar){
|
||||
for(i=0; i<m; ++i){
|
||||
for(j=0; j<m; ++j)
|
||||
covar[i*m+j]=covext[i*mm+j];
|
||||
}
|
||||
}
|
||||
|
||||
free(ptr);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/* convenience wrappers to LEVMAR_BLEIC_DER/LEVMAR_BLEIC_DIF */
|
||||
|
||||
/* box & linear inequality constraints */
|
||||
int LEVMAR_BLIC_DER(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *lb, LM_REAL *ub,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
int LEVMAR_BLIC_DIF(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *lb, LM_REAL *ub,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DIF(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
/* linear equation & inequality constraints */
|
||||
int LEVMAR_LEIC_DER(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *A, LM_REAL *b, int k1,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
int LEVMAR_LEIC_DIF(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *A, LM_REAL *b, int k1,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, A, b, k1, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
/* linear inequality constraints */
|
||||
int LEVMAR_LIC_DER(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[4], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DER(func, jacf, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
int LEVMAR_LIC_DIF(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n,
|
||||
LM_REAL *C, LM_REAL *d, int k2,
|
||||
int itmax, LM_REAL opts[5], LM_REAL info[LM_INFO_SZ], LM_REAL *work, LM_REAL *covar, void *adata)
|
||||
{
|
||||
return LEVMAR_BLEIC_DIF(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, k2, itmax, opts, info, work, covar, adata);
|
||||
}
|
||||
|
||||
/* undefine all. THIS MUST REMAIN AT THE END OF THE FILE */
|
||||
#undef LMBLEIC_DATA
|
||||
#undef LMBLEIC_ELIM
|
||||
#undef LMBLEIC_FUNC
|
||||
#undef LMBLEIC_JACF
|
||||
#undef LEVMAR_FDIF_FORW_JAC_APPROX
|
||||
#undef LEVMAR_COVAR
|
||||
#undef LEVMAR_TRANS_MAT_MAT_MULT
|
||||
#undef LEVMAR_BLEIC_DER
|
||||
#undef LEVMAR_BLEIC_DIF
|
||||
#undef LEVMAR_BLIC_DER
|
||||
#undef LEVMAR_BLIC_DIF
|
||||
#undef LEVMAR_LEIC_DER
|
||||
#undef LEVMAR_LEIC_DIF
|
||||
#undef LEVMAR_LIC_DER
|
||||
#undef LEVMAR_LIC_DIF
|
||||
#undef LEVMAR_BLEC_DER
|
||||
#undef LEVMAR_BLEC_DIF
|
||||
@@ -1,80 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*******************************************************************************
|
||||
* Wrappers for linearly constrained Levenberg-Marquardt minimization. The same
|
||||
* core code is used with appropriate #defines to derive single and double
|
||||
* precision versions, see also lmlec_core.c
|
||||
*******************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "misc.h"
|
||||
|
||||
|
||||
#ifndef HAVE_LAPACK
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("Linearly constrained optimization requires LAPACK and was not compiled!")
|
||||
#else
|
||||
#warning Linearly constrained optimization requires LAPACK and was not compiled!
|
||||
#endif // _MSC_VER
|
||||
|
||||
#else // LAPACK present
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "lmlec_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "lmlec_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
@@ -1,656 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef LM_REAL // not included by lmlec.c
|
||||
#error This file should not be compiled directly!
|
||||
#endif
|
||||
|
||||
|
||||
/* precision-specific definitions */
|
||||
#define LMLEC_DATA LM_ADD_PREFIX(lmlec_data)
|
||||
#define LMLEC_ELIM LM_ADD_PREFIX(lmlec_elim)
|
||||
#define LMLEC_FUNC LM_ADD_PREFIX(lmlec_func)
|
||||
#define LMLEC_JACF LM_ADD_PREFIX(lmlec_jacf)
|
||||
#define LEVMAR_LEC_DER LM_ADD_PREFIX(levmar_lec_der)
|
||||
#define LEVMAR_LEC_DIF LM_ADD_PREFIX(levmar_lec_dif)
|
||||
#define LEVMAR_DER LM_ADD_PREFIX(levmar_der)
|
||||
#define LEVMAR_DIF LM_ADD_PREFIX(levmar_dif)
|
||||
#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
|
||||
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
||||
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
|
||||
|
||||
#define GEQP3 LM_MK_LAPACK_NAME(geqp3)
|
||||
#define ORGQR LM_MK_LAPACK_NAME(orgqr)
|
||||
#define TRTRI LM_MK_LAPACK_NAME(trtri)
|
||||
|
||||
struct LMLEC_DATA{
|
||||
LM_REAL *c, *Z, *p, *jac;
|
||||
int ncnstr;
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata);
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *jac, int m, int n, void *adata);
|
||||
void *adata;
|
||||
};
|
||||
|
||||
/* prototypes for LAPACK routines */
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
extern int GEQP3(int *m, int *n, LM_REAL *a, int *lda, int *jpvt,
|
||||
LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
|
||||
|
||||
extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau,
|
||||
LM_REAL *work, int *lwork, int *info);
|
||||
|
||||
extern int TRTRI(char *uplo, char *diag, int *n, LM_REAL *a, int *lda, int *info);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* This function implements an elimination strategy for linearly constrained
|
||||
* optimization problems. The strategy relies on QR decomposition to transform
|
||||
* an optimization problem constrained by Ax=b to an equivalent, unconstrained
|
||||
* one. Also referred to as "null space" or "reduced Hessian" method.
|
||||
* See pp. 430-433 (chap. 15) of "Numerical Optimization" by Nocedal-Wright
|
||||
* for details.
|
||||
*
|
||||
* A is mxn with m<=n and rank(A)=m
|
||||
* Two matrices Y and Z of dimensions nxm and nx(n-m) are computed from A^T so that
|
||||
* their columns are orthonormal and every x can be written as x=Y*b + Z*x_z=
|
||||
* c + Z*x_z, where c=Y*b is a fixed vector of dimension n and x_z is an
|
||||
* arbitrary vector of dimension n-m. Then, the problem of minimizing f(x)
|
||||
* subject to Ax=b is equivalent to minimizing f(c + Z*x_z) with no constraints.
|
||||
* The computed Y and Z are such that any solution of Ax=b can be written as
|
||||
* x=Y*x_y + Z*x_z for some x_y, x_z. Furthermore, A*Y is nonsingular, A*Z=0
|
||||
* and Z spans the null space of A.
|
||||
*
|
||||
* The function accepts A, b and computes c, Y, Z. If b or c is NULL, c is not
|
||||
* computed. Also, Y can be NULL in which case it is not referenced.
|
||||
* The function returns LM_ERROR in case of error, A's computed rank if successful
|
||||
*
|
||||
*/
|
||||
static int LMLEC_ELIM(LM_REAL *A, LM_REAL *b, LM_REAL *c, LM_REAL *Y, LM_REAL *Z, int m, int n)
|
||||
{
|
||||
static LM_REAL eps=LM_CNST(-1.0);
|
||||
|
||||
LM_REAL *buf=NULL;
|
||||
LM_REAL *a, *tau, *work, *r, aux;
|
||||
register LM_REAL tmp;
|
||||
int a_sz, jpvt_sz, tau_sz, r_sz, Y_sz, worksz;
|
||||
int info, rank, *jpvt, tot_sz, mintmn, tm, tn;
|
||||
register int i, j, k;
|
||||
|
||||
if(m>n){
|
||||
fprintf(stderr, RCAT("matrix of constraints cannot have more rows than columns in", LMLEC_ELIM) "()!\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
tm=n; tn=m; // transpose dimensions
|
||||
mintmn=m;
|
||||
|
||||
/* calculate required memory size */
|
||||
worksz=-1; // workspace query. Optimal work size is returned in aux
|
||||
//ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, NULL, (int *)&tm, NULL, (LM_REAL *)&aux, &worksz, &info);
|
||||
GEQP3((int *)&tm, (int *)&tn, NULL, (int *)&tm, NULL, NULL, (LM_REAL *)&aux, (int *)&worksz, &info);
|
||||
worksz=(int)aux;
|
||||
a_sz=tm*tm; // tm*tn is enough for xgeqp3()
|
||||
jpvt_sz=tn;
|
||||
tau_sz=mintmn;
|
||||
r_sz=mintmn*mintmn; // actually smaller if a is not of full row rank
|
||||
Y_sz=(Y)? 0 : tm*tn;
|
||||
|
||||
tot_sz=(a_sz + tau_sz + r_sz + worksz + Y_sz)*sizeof(LM_REAL) + jpvt_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
|
||||
buf=(LM_REAL *)malloc(tot_sz); /* allocate a "big" memory chunk at once */
|
||||
if(!buf){
|
||||
fprintf(stderr, RCAT("Memory allocation request failed in ", LMLEC_ELIM) "()\n");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
a=buf;
|
||||
tau=a+a_sz;
|
||||
r=tau+tau_sz;
|
||||
work=r+r_sz;
|
||||
if(!Y){
|
||||
Y=work+worksz;
|
||||
jpvt=(int *)(Y+Y_sz);
|
||||
}
|
||||
else
|
||||
jpvt=(int *)(work+worksz);
|
||||
|
||||
/* copy input array so that LAPACK won't destroy it. Note that copying is
|
||||
* done in row-major order, which equals A^T in column-major
|
||||
*/
|
||||
for(i=0; i<tm*tn; ++i)
|
||||
a[i]=A[i];
|
||||
|
||||
/* clear jpvt */
|
||||
for(i=0; i<jpvt_sz; ++i) jpvt[i]=0;
|
||||
|
||||
/* rank revealing QR decomposition of A^T*/
|
||||
GEQP3((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, (int *)&worksz, &info);
|
||||
//dgeqpf_((int *)&tm, (int *)&tn, a, (int *)&tm, jpvt, tau, work, &info);
|
||||
/* error checking */
|
||||
if(info!=0){
|
||||
if(info<0){
|
||||
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQP3) " in ", LMLEC_ELIM) "()\n", -info);
|
||||
}
|
||||
else if(info>0){
|
||||
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", GEQP3) " in ", LMLEC_ELIM) "()\n", info);
|
||||
}
|
||||
free(buf);
|
||||
return LM_ERROR;
|
||||
}
|
||||
/* the upper triangular part of a now contains the upper triangle of the unpermuted R */
|
||||
|
||||
if(eps<0.0){
|
||||
LM_REAL aux;
|
||||
|
||||
/* compute machine epsilon. DBL_EPSILON should do also */
|
||||
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
|
||||
;
|
||||
eps*=LM_CNST(2.0);
|
||||
}
|
||||
|
||||
tmp=tm*LM_CNST(10.0)*eps*FABS(a[0]); // threshold. tm is max(tm, tn)
|
||||
tmp=(tmp>LM_CNST(1E-12))? tmp : LM_CNST(1E-12); // ensure that threshold is not too small
|
||||
/* compute A^T's numerical rank by counting the non-zeros in R's diagonal */
|
||||
for(i=rank=0; i<mintmn; ++i)
|
||||
if(a[i*(tm+1)]>tmp || a[i*(tm+1)]<-tmp) ++rank; /* loop across R's diagonal elements */
|
||||
else break; /* diagonal is arranged in absolute decreasing order */
|
||||
|
||||
if(rank<tn){
|
||||
fprintf(stderr, RCAT("\nConstraints matrix in ", LMLEC_ELIM) "() is not of full row rank (i.e. %d < %d)!\n"
|
||||
"Make sure that you do not specify redundant or inconsistent constraints.\n\n", rank, tn);
|
||||
free(buf);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* compute the permuted inverse transpose of R */
|
||||
/* first, copy R from the upper triangular part of a to the lower part of r (thus transposing it). R is rank x rank */
|
||||
for(j=0; j<rank; ++j){
|
||||
for(i=0; i<=j; ++i)
|
||||
r[j+i*rank]=a[i+j*tm];
|
||||
for(i=j+1; i<rank; ++i)
|
||||
r[j+i*rank]=0.0; // upper part is zero
|
||||
}
|
||||
/* r now contains R^T */
|
||||
|
||||
/* compute the inverse */
|
||||
TRTRI("L", "N", (int *)&rank, r, (int *)&rank, &info);
|
||||
/* error checking */
|
||||
if(info!=0){
|
||||
if(info<0){
|
||||
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRI) " in ", LMLEC_ELIM) "()\n", -info);
|
||||
}
|
||||
else if(info>0){
|
||||
fprintf(stderr, RCAT(RCAT("A(%d, %d) is exactly zero for ", TRTRI) " (singular matrix) in ", LMLEC_ELIM) "()\n", info, info);
|
||||
}
|
||||
free(buf);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* finally, permute R^-T using Y as intermediate storage */
|
||||
for(j=0; j<rank; ++j)
|
||||
for(i=0, k=jpvt[j]-1; i<rank; ++i)
|
||||
Y[i+k*rank]=r[i+j*rank];
|
||||
|
||||
for(i=0; i<rank*rank; ++i) // copy back to r
|
||||
r[i]=Y[i];
|
||||
|
||||
/* resize a to be tm x tm, filling with zeroes */
|
||||
for(i=tm*tn; i<tm*tm; ++i)
|
||||
a[i]=0.0;
|
||||
|
||||
/* compute Q in a as the product of elementary reflectors. Q is tm x tm */
|
||||
ORGQR((int *)&tm, (int *)&tm, (int *)&mintmn, a, (int *)&tm, tau, work, &worksz, &info);
|
||||
/* error checking */
|
||||
if(info!=0){
|
||||
if(info<0){
|
||||
fprintf(stderr, RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", LMLEC_ELIM) "()\n", -info);
|
||||
}
|
||||
else if(info>0){
|
||||
fprintf(stderr, RCAT(RCAT("unknown LAPACK error (%d) for ", ORGQR) " in ", LMLEC_ELIM) "()\n", info);
|
||||
}
|
||||
free(buf);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* compute Y=Q_1*R^-T*P^T. Y is tm x rank */
|
||||
for(i=0; i<tm; ++i)
|
||||
for(j=0; j<rank; ++j){
|
||||
for(k=0, tmp=0.0; k<rank; ++k)
|
||||
tmp+=a[i+k*tm]*r[k+j*rank];
|
||||
Y[i*rank+j]=tmp;
|
||||
}
|
||||
|
||||
if(b && c){
|
||||
/* compute c=Y*b */
|
||||
for(i=0; i<tm; ++i){
|
||||
for(j=0, tmp=0.0; j<rank; ++j)
|
||||
tmp+=Y[i*rank+j]*b[j];
|
||||
|
||||
c[i]=tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/* copy Q_2 into Z. Z is tm x (tm-rank) */
|
||||
for(j=0; j<tm-rank; ++j)
|
||||
for(i=0, k=j+rank; i<tm; ++i)
|
||||
Z[i*(tm-rank)+j]=a[i+k*tm];
|
||||
|
||||
free(buf);
|
||||
|
||||
return rank;
|
||||
}
|
||||
|
||||
/* constrained measurements: given pp, compute the measurements at c + Z*pp */
|
||||
static void LMLEC_FUNC(LM_REAL *pp, LM_REAL *hx, int mm, int n, void *adata)
|
||||
{
|
||||
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
|
||||
int m;
|
||||
register int i, j;
|
||||
register LM_REAL sum;
|
||||
LM_REAL *c, *Z, *p, *Zimm;
|
||||
|
||||
m=mm+data->ncnstr;
|
||||
c=data->c;
|
||||
Z=data->Z;
|
||||
p=data->p;
|
||||
/* p=c + Z*pp */
|
||||
for(i=0; i<m; ++i){
|
||||
Zimm=Z+i*mm;
|
||||
for(j=0, sum=c[i]; j<mm; ++j)
|
||||
sum+=Zimm[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
|
||||
p[i]=sum;
|
||||
}
|
||||
|
||||
(*(data->func))(p, hx, m, n, data->adata);
|
||||
}
|
||||
|
||||
/* constrained Jacobian: given pp, compute the Jacobian at c + Z*pp
|
||||
* Using the chain rule, the Jacobian with respect to pp equals the
|
||||
* product of the Jacobian with respect to p (at c + Z*pp) times Z
|
||||
*/
|
||||
static void LMLEC_JACF(LM_REAL *pp, LM_REAL *jacjac, int mm, int n, void *adata)
|
||||
{
|
||||
struct LMLEC_DATA *data=(struct LMLEC_DATA *)adata;
|
||||
int m;
|
||||
register int i, j, l;
|
||||
register LM_REAL sum, *aux1, *aux2;
|
||||
LM_REAL *c, *Z, *p, *jac;
|
||||
|
||||
m=mm+data->ncnstr;
|
||||
c=data->c;
|
||||
Z=data->Z;
|
||||
p=data->p;
|
||||
jac=data->jac;
|
||||
/* p=c + Z*pp */
|
||||
for(i=0; i<m; ++i){
|
||||
aux1=Z+i*mm;
|
||||
for(j=0, sum=c[i]; j<mm; ++j)
|
||||
sum+=aux1[j]*pp[j]; // sum+=Z[i*mm+j]*pp[j];
|
||||
p[i]=sum;
|
||||
}
|
||||
|
||||
(*(data->jacf))(p, jac, m, n, data->adata);
|
||||
|
||||
/* compute jac*Z in jacjac */
|
||||
if(n*m<=__BLOCKSZ__SQ){ // this is a small problem
|
||||
/* This is the straightforward way to compute jac*Z. However, due to
|
||||
* its noncontinuous memory access pattern, it incures many cache misses when
|
||||
* applied to large minimization problems (i.e. problems involving a large
|
||||
* number of free variables and measurements), in which jac is too large to
|
||||
* 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. NULL implies a zero 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 constraints (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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_LEC_DER_WORKSZ() reals large, allocated 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");
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
mm=m-k;
|
||||
|
||||
if(n<mm){
|
||||
fprintf(stderr, LCAT(LEVMAR_LEC_DER, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
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"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
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;
|
||||
|
||||
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
|
||||
if(ret==LM_ERROR){
|
||||
free(ptr);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* 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])>LM_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){
|
||||
LEVMAR_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. NULL implies a zero 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 constraints (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
|
||||
* 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
|
||||
* info[7]= # function evaluations
|
||||
* info[8]= # Jacobian evaluations
|
||||
* info[9]= # linear systems solved, i.e. # attempts for reducing error
|
||||
*/
|
||||
LM_REAL *work, /* working memory at least LM_LEC_DIF_WORKSZ() reals large, allocated 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;
|
||||
|
||||
if(n<mm){
|
||||
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): cannot solve a problem with fewer measurements + equality constraints [%d + %d] than unknowns [%d]\n"), n, k, m);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
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"));
|
||||
return LM_ERROR;
|
||||
}
|
||||
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;
|
||||
|
||||
ret=LMLEC_ELIM(A, b, data.c, NULL, Z, k, m); // compute c, Z
|
||||
if(ret==LM_ERROR){
|
||||
free(ptr);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* 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])>LM_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(!hx){
|
||||
fprintf(stderr, LCAT(LEVMAR_LEC_DIF, "(): memory allocation request failed\n"));
|
||||
free(ptr);
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
wrk=hx+n;
|
||||
jac=wrk+n;
|
||||
|
||||
(*func)(p, hx, m, n, adata); /* evaluate function at p */
|
||||
LEVMAR_FDIF_FORW_JAC_APPROX(func, p, hx, wrk, (LM_REAL)LM_DIFF_DELTA, jac, m, n, adata); /* compute the Jacobian at p */
|
||||
LEVMAR_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 LEVMAR_FDIF_FORW_JAC_APPROX
|
||||
#undef LEVMAR_COVAR
|
||||
#undef LEVMAR_TRANS_MAT_MAT_MULT
|
||||
#undef LEVMAR_LEC_DER
|
||||
#undef LEVMAR_LEC_DIF
|
||||
#undef LEVMAR_DER
|
||||
#undef LEVMAR_DIF
|
||||
|
||||
#undef GEQP3
|
||||
#undef ORGQR
|
||||
#undef TRTRI
|
||||
@@ -1,70 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/********************************************************************************
|
||||
* Miscelaneous functions for Levenberg-Marquardt nonlinear minimization. The
|
||||
* same core code is used with appropriate #defines to derive single and double
|
||||
* precision versions, see also misc_core.c
|
||||
********************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include "levmar.h"
|
||||
#include "misc.h"
|
||||
|
||||
#if !defined(LM_DBL_PREC) && !defined(LM_SNGL_PREC)
|
||||
#error At least one of LM_DBL_PREC, LM_SNGL_PREC should be defined!
|
||||
#endif
|
||||
|
||||
#ifdef LM_SNGL_PREC
|
||||
/* single precision (float) definitions */
|
||||
#define LM_REAL float
|
||||
#define LM_PREFIX s
|
||||
|
||||
#define LM_REAL_EPSILON FLT_EPSILON
|
||||
#define __SUBCNST(x) x##F
|
||||
#define LM_CNST(x) __SUBCNST(x) // force substitution
|
||||
|
||||
#include "misc_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef __SUBCNST
|
||||
#undef LM_CNST
|
||||
#endif /* LM_SNGL_PREC */
|
||||
|
||||
#ifdef LM_DBL_PREC
|
||||
/* double precision definitions */
|
||||
#define LM_REAL double
|
||||
#define LM_PREFIX d
|
||||
|
||||
#define LM_REAL_EPSILON DBL_EPSILON
|
||||
#define LM_CNST(x) (x)
|
||||
|
||||
#include "misc_core.c" // read in core code
|
||||
|
||||
#undef LM_REAL
|
||||
#undef LM_PREFIX
|
||||
#undef LM_REAL_EPSILON
|
||||
#undef LM_CNST
|
||||
#endif /* LM_DBL_PREC */
|
||||
@@ -1,114 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef _MISC_H_
|
||||
#define _MISC_H_
|
||||
|
||||
/* common suffix for LAPACK subroutines. Define empty in case of no prefix. */
|
||||
#define LM_LAPACK_SUFFIX _
|
||||
//#define LM_LAPACK_SUFFIX // define empty
|
||||
|
||||
/* common prefix for BLAS subroutines. Leave undefined in case of no prefix.
|
||||
* You might also need to modify LM_BLAS_PREFIX below
|
||||
*/
|
||||
/* f2c'd BLAS */
|
||||
//#define LM_BLAS_PREFIX f2c_
|
||||
/* C BLAS */
|
||||
//#define LM_BLAS_PREFIX cblas_
|
||||
|
||||
/* common suffix for BLAS subroutines */
|
||||
//#define LM_BLAS_SUFFIX // define empty if a f2c_ or cblas_ prefix was defined for LM_BLAS_PREFIX above
|
||||
#define LM_BLAS_SUFFIX _ // use this in case of no BLAS prefix
|
||||
|
||||
|
||||
#define LCAT_(a, b) #a b
|
||||
#define LCAT(a, b) LCAT_(a, b) // force substitution
|
||||
#define RCAT_(a, b) a #b
|
||||
#define RCAT(a, b) RCAT_(a, b) // force substitution
|
||||
|
||||
#define LM_MK_LAPACK_NAME(s) LM_ADD_PREFIX(LM_CAT_(s, LM_LAPACK_SUFFIX))
|
||||
|
||||
#ifdef LM_BLAS_PREFIX
|
||||
#define LM_MK_BLAS_NAME(s) LM_CAT_(LM_BLAS_PREFIX, LM_ADD_PREFIX(LM_CAT_(s, LM_BLAS_SUFFIX)))
|
||||
#else
|
||||
#define LM_MK_BLAS_NAME(s) LM_ADD_PREFIX(LM_CAT_(s, LM_BLAS_SUFFIX))
|
||||
#endif
|
||||
|
||||
|
||||
#define __BLOCKSZ__ 32 /* block size for cache-friendly matrix-matrix multiply. It should be
|
||||
* such that __BLOCKSZ__^2*sizeof(LM_REAL) is smaller than the CPU (L1)
|
||||
* data cache size. Notice that a value of 32 when LM_REAL=double assumes
|
||||
* an 8Kb L1 data cache (32*32*8=8K). This is a concervative choice since
|
||||
* newer Pentium 4s have a L1 data cache of size 16K, capable of holding
|
||||
* up to 45x45 double blocks.
|
||||
*/
|
||||
#define __BLOCKSZ__SQ (__BLOCKSZ__)*(__BLOCKSZ__)
|
||||
|
||||
/* add a prefix in front of a token */
|
||||
#define LM_CAT__(a, b) a ## b
|
||||
#define LM_CAT_(a, b) LM_CAT__(a, b) // force substitution
|
||||
#define LM_ADD_PREFIX(s) LM_CAT_(LM_PREFIX, s)
|
||||
|
||||
#define FABS(x) (((x)>=0.0)? (x) : -(x))
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* blocking-based matrix multiply */
|
||||
extern void slevmar_trans_mat_mat_mult(float *a, float *b, int n, int m);
|
||||
extern void dlevmar_trans_mat_mat_mult(double *a, double *b, int n, int m);
|
||||
|
||||
/* forward finite differences */
|
||||
extern void slevmar_fdif_forw_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *hx, float *hxx, float delta,
|
||||
float *jac, int m, int n, void *adata);
|
||||
extern void dlevmar_fdif_forw_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *hx, double *hxx, double delta,
|
||||
double *jac, int m, int n, void *adata);
|
||||
|
||||
/* central finite differences */
|
||||
extern void slevmar_fdif_cent_jac_approx(void (*func)(float *p, float *hx, int m, int n, void *adata),
|
||||
float *p, float *hxm, float *hxp, float delta,
|
||||
float *jac, int m, int n, void *adata);
|
||||
extern void dlevmar_fdif_cent_jac_approx(void (*func)(double *p, double *hx, int m, int n, void *adata),
|
||||
double *p, double *hxm, double *hxp, double delta,
|
||||
double *jac, int m, int n, void *adata);
|
||||
|
||||
/* e=x-y and ||e|| */
|
||||
extern float slevmar_L2nrmxmy(float *e, float *x, float *y, int n);
|
||||
extern double dlevmar_L2nrmxmy(double *e, double *x, double *y, int n);
|
||||
|
||||
/* covariance of LS fit */
|
||||
extern int slevmar_covar(float *JtJ, float *C, float sumsq, int m, int n);
|
||||
extern int dlevmar_covar(double *JtJ, double *C, double sumsq, int m, int n);
|
||||
|
||||
/* box constraints consistency check */
|
||||
extern int slevmar_box_check(float *lb, float *ub, int m);
|
||||
extern int dlevmar_box_check(double *lb, double *ub, int m);
|
||||
|
||||
/* Cholesky */
|
||||
extern int slevmar_chol(float *C, float *W, int m);
|
||||
extern int dlevmar_chol(double *C, double *W, int m);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _MISC_H_ */
|
||||
@@ -1,831 +0,0 @@
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Levenberg - Marquardt non-linear minimization algorithm
|
||||
// Copyright (C) 2004-05 Manolis Lourakis (lourakis at ics forth gr)
|
||||
// Institute of Computer Science, Foundation for Research & Technology - Hellas
|
||||
// Heraklion, Crete, Greece.
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifndef LM_REAL // not included by misc.c
|
||||
#error This file should not be compiled directly!
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic ignored "-Wimplicit-fallthrough"
|
||||
#pragma GCC diagnostic ignored "-Wcpp"
|
||||
#endif
|
||||
|
||||
/* precision-specific definitions */
|
||||
#define LEVMAR_CHKJAC LM_ADD_PREFIX(levmar_chkjac)
|
||||
#define LEVMAR_FDIF_FORW_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_forw_jac_approx)
|
||||
#define LEVMAR_FDIF_CENT_JAC_APPROX LM_ADD_PREFIX(levmar_fdif_cent_jac_approx)
|
||||
#define LEVMAR_TRANS_MAT_MAT_MULT LM_ADD_PREFIX(levmar_trans_mat_mat_mult)
|
||||
#define LEVMAR_COVAR LM_ADD_PREFIX(levmar_covar)
|
||||
#define LEVMAR_STDDEV LM_ADD_PREFIX(levmar_stddev)
|
||||
#define LEVMAR_CORCOEF LM_ADD_PREFIX(levmar_corcoef)
|
||||
#define LEVMAR_R2 LM_ADD_PREFIX(levmar_R2)
|
||||
#define LEVMAR_BOX_CHECK LM_ADD_PREFIX(levmar_box_check)
|
||||
#define LEVMAR_L2NRMXMY LM_ADD_PREFIX(levmar_L2nrmxmy)
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
#define LEVMAR_PSEUDOINVERSE LM_ADD_PREFIX(levmar_pseudoinverse)
|
||||
static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m);
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
/* BLAS matrix multiplication, LAPACK SVD & Cholesky routines */
|
||||
#define GEMM LM_MK_BLAS_NAME(gemm)
|
||||
/* C := alpha*op( A )*op( B ) + beta*C */
|
||||
extern void GEMM(char *transa, char *transb, int *m, int *n, int *k,
|
||||
LM_REAL *alpha, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, LM_REAL *beta, LM_REAL *c, int *ldc);
|
||||
|
||||
#define GESVD LM_MK_LAPACK_NAME(gesvd)
|
||||
#define GESDD LM_MK_LAPACK_NAME(gesdd)
|
||||
extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,
|
||||
LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);
|
||||
|
||||
/* lapack 3.0 new SVD routine, faster than xgesvd() */
|
||||
extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,
|
||||
LM_REAL *work, int *lwork, int *iwork, int *info);
|
||||
|
||||
/* Cholesky decomposition */
|
||||
#define POTF2 LM_MK_LAPACK_NAME(potf2)
|
||||
extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#define LEVMAR_CHOLESKY LM_ADD_PREFIX(levmar_chol)
|
||||
|
||||
#else /* !HAVE_LAPACK */
|
||||
#define LEVMAR_LUINVERSE LM_ADD_PREFIX(levmar_LUinverse_noLapack)
|
||||
|
||||
static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m);
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
/* blocked multiplication of the transpose of the nxm matrix a with itself (i.e. a^T a)
|
||||
* using a block size of bsize. The product is returned in b.
|
||||
* Since a^T a is symmetric, its computation can be sped up by computing only its
|
||||
* upper triangular part and copying it to the lower part.
|
||||
*
|
||||
* More details on blocking can be found at
|
||||
* http://www-2.cs.cmu.edu/afs/cs/academic/class/15213-f02/www/R07/section_a/Recitation07-SectionA.pdf
|
||||
*/
|
||||
void LEVMAR_TRANS_MAT_MAT_MULT(LM_REAL *a, LM_REAL *b, int n, int m)
|
||||
{
|
||||
#ifdef HAVE_LAPACK /* use BLAS matrix multiply */
|
||||
|
||||
LM_REAL alpha=LM_CNST(1.0), beta=LM_CNST(0.0);
|
||||
/* Fool BLAS to compute a^T*a avoiding transposing a: a is equivalent to a^T in column major,
|
||||
* therefore BLAS computes a*a^T with a and a*a^T in column major, which is equivalent to
|
||||
* computing a^T*a in row major!
|
||||
*/
|
||||
GEMM("N", "T", &m, &m, &n, &alpha, a, &m, a, &m, &beta, b, &m);
|
||||
|
||||
#else /* no LAPACK, use blocking-based multiply */
|
||||
|
||||
register int i, j, k, jj, kk;
|
||||
register LM_REAL sum, *bim, *akm;
|
||||
const int bsize=__BLOCKSZ__;
|
||||
|
||||
#define __MIN__(x, y) (((x)<=(y))? (x) : (y))
|
||||
#define __MAX__(x, y) (((x)>=(y))? (x) : (y))
|
||||
|
||||
/* compute upper triangular part using blocking */
|
||||
for(jj=0; jj<m; jj+=bsize){
|
||||
for(i=0; i<m; ++i){
|
||||
bim=b+i*m;
|
||||
for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j)
|
||||
bim[j]=0.0; //b[i*m+j]=0.0;
|
||||
}
|
||||
|
||||
for(kk=0; kk<n; kk+=bsize){
|
||||
for(i=0; i<m; ++i){
|
||||
bim=b+i*m;
|
||||
for(j=__MAX__(jj, i); j<__MIN__(jj+bsize, m); ++j){
|
||||
sum=0.0;
|
||||
for(k=kk; k<__MIN__(kk+bsize, n); ++k){
|
||||
akm=a+k*m;
|
||||
sum+=akm[i]*akm[j]; //a[k*m+i]*a[k*m+j];
|
||||
}
|
||||
bim[j]+=sum; //b[i*m+j]+=sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* copy upper triangular part to the lower one */
|
||||
for(i=0; i<m; ++i)
|
||||
for(j=0; j<i; ++j)
|
||||
b[i*m+j]=b[j*m+i];
|
||||
|
||||
#undef __MIN__
|
||||
#undef __MAX__
|
||||
|
||||
#endif /* HAVE_LAPACK */
|
||||
}
|
||||
|
||||
/* forward finite difference approximation to the Jacobian of func */
|
||||
void LEVMAR_FDIF_FORW_JAC_APPROX(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
/* function to differentiate */
|
||||
LM_REAL *p, /* I: current parameter estimate, mx1 */
|
||||
LM_REAL *hx, /* I: func evaluated at p, i.e. hx=func(p), nx1 */
|
||||
LM_REAL *hxx, /* W/O: work array for evaluating func(p+delta), nx1 */
|
||||
LM_REAL delta, /* increment for computing the Jacobian */
|
||||
LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */
|
||||
int m,
|
||||
int n,
|
||||
void *adata)
|
||||
{
|
||||
register int i, j;
|
||||
LM_REAL tmp;
|
||||
register LM_REAL d;
|
||||
|
||||
for(j=0; j<m; ++j){
|
||||
/* determine d=max(1E-04*|p[j]|, delta), see HZ */
|
||||
d=LM_CNST(1E-04)*p[j]; // force evaluation
|
||||
d=FABS(d);
|
||||
if(d<delta)
|
||||
d=delta;
|
||||
|
||||
tmp=p[j];
|
||||
p[j]+=d;
|
||||
|
||||
(*func)(p, hxx, m, n, adata);
|
||||
|
||||
p[j]=tmp; /* restore */
|
||||
|
||||
d=LM_CNST(1.0)/d; /* invert so that divisions can be carried out faster as multiplications */
|
||||
for(i=0; i<n; ++i){
|
||||
jac[i*m+j]=(hxx[i]-hx[i])*d;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* central finite difference approximation to the Jacobian of func */
|
||||
void LEVMAR_FDIF_CENT_JAC_APPROX(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
/* function to differentiate */
|
||||
LM_REAL *p, /* I: current parameter estimate, mx1 */
|
||||
LM_REAL *hxm, /* W/O: work array for evaluating func(p-delta), nx1 */
|
||||
LM_REAL *hxp, /* W/O: work array for evaluating func(p+delta), nx1 */
|
||||
LM_REAL delta, /* increment for computing the Jacobian */
|
||||
LM_REAL *jac, /* O: array for storing approximated Jacobian, nxm */
|
||||
int m,
|
||||
int n,
|
||||
void *adata)
|
||||
{
|
||||
register int i, j;
|
||||
LM_REAL tmp;
|
||||
register LM_REAL d;
|
||||
|
||||
for(j=0; j<m; ++j){
|
||||
/* determine d=max(1E-04*|p[j]|, delta), see HZ */
|
||||
d=LM_CNST(1E-04)*p[j]; // force evaluation
|
||||
d=FABS(d);
|
||||
if(d<delta)
|
||||
d=delta;
|
||||
|
||||
tmp=p[j];
|
||||
p[j]-=d;
|
||||
(*func)(p, hxm, m, n, adata);
|
||||
|
||||
p[j]=tmp+d;
|
||||
(*func)(p, hxp, m, n, adata);
|
||||
p[j]=tmp; /* restore */
|
||||
|
||||
d=LM_CNST(0.5)/d; /* invert so that divisions can be carried out faster as multiplications */
|
||||
for(i=0; i<n; ++i){
|
||||
jac[i*m+j]=(hxp[i]-hxm[i])*d;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Check the Jacobian of a n-valued nonlinear function in m variables
|
||||
* evaluated at a point p, for consistency with the function itself.
|
||||
*
|
||||
* Based on fortran77 subroutine CHKDER by
|
||||
* Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
|
||||
* Argonne National Laboratory. MINPACK project. March 1980.
|
||||
*
|
||||
*
|
||||
* func points to a function from R^m --> R^n: Given a p in R^m it yields hx in R^n
|
||||
* jacf points to a function implementing the Jacobian of func, whose correctness
|
||||
* is to be tested. Given a p in R^m, jacf computes into the nxm matrix j the
|
||||
* Jacobian of func at p. Note that row i of j corresponds to the gradient of
|
||||
* the i-th component of func, evaluated at p.
|
||||
* p is an input array of length m containing the point of evaluation.
|
||||
* m is the number of variables
|
||||
* n is the number of functions
|
||||
* adata points to possible additional data and is passed uninterpreted
|
||||
* to func, jacf.
|
||||
* err is an array of length n. On output, err contains measures
|
||||
* of correctness of the respective gradients. if there is
|
||||
* no severe loss of significance, then if err[i] is 1.0 the
|
||||
* i-th gradient is correct, while if err[i] is 0.0 the i-th
|
||||
* gradient is incorrect. For values of err between 0.0 and 1.0,
|
||||
* the categorization is less certain. In general, a value of
|
||||
* err[i] greater than 0.5 indicates that the i-th gradient is
|
||||
* probably correct, while a value of err[i] less than 0.5
|
||||
* indicates that the i-th gradient is probably incorrect.
|
||||
*
|
||||
*
|
||||
* The function does not perform reliably if cancellation or
|
||||
* rounding errors cause a severe loss of significance in the
|
||||
* evaluation of a function. therefore, none of the components
|
||||
* of p should be unusually small (in particular, zero) or any
|
||||
* other value which may cause loss of significance.
|
||||
*/
|
||||
|
||||
void LEVMAR_CHKJAC(
|
||||
void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
void (*jacf)(LM_REAL *p, LM_REAL *j, int m, int n, void *adata),
|
||||
LM_REAL *p, int m, int n, void *adata, LM_REAL *err)
|
||||
{
|
||||
LM_REAL factor=LM_CNST(100.0);
|
||||
LM_REAL one=LM_CNST(1.0);
|
||||
LM_REAL zero=LM_CNST(0.0);
|
||||
LM_REAL *fvec, *fjac, *pp, *fvecp, *buf;
|
||||
|
||||
register int i, j;
|
||||
LM_REAL eps, epsf, temp, epsmch;
|
||||
LM_REAL epslog;
|
||||
int fvec_sz=n, fjac_sz=n*m, pp_sz=m, fvecp_sz=n;
|
||||
|
||||
epsmch=LM_REAL_EPSILON;
|
||||
eps=(LM_REAL)sqrt(epsmch);
|
||||
|
||||
buf=(LM_REAL *)malloc((fvec_sz + fjac_sz + pp_sz + fvecp_sz)*sizeof(LM_REAL));
|
||||
if(!buf){
|
||||
fprintf(stderr, LCAT(LEVMAR_CHKJAC, "(): memory allocation request failed\n"));
|
||||
exit(1);
|
||||
}
|
||||
fvec=buf;
|
||||
fjac=fvec+fvec_sz;
|
||||
pp=fjac+fjac_sz;
|
||||
fvecp=pp+pp_sz;
|
||||
|
||||
/* compute fvec=func(p) */
|
||||
(*func)(p, fvec, m, n, adata);
|
||||
|
||||
/* compute the Jacobian at p */
|
||||
(*jacf)(p, fjac, m, n, adata);
|
||||
|
||||
/* compute pp */
|
||||
for(j=0; j<m; ++j){
|
||||
temp=eps*FABS(p[j]);
|
||||
if(temp==zero) temp=eps;
|
||||
pp[j]=p[j]+temp;
|
||||
}
|
||||
|
||||
/* compute fvecp=func(pp) */
|
||||
(*func)(pp, fvecp, m, n, adata);
|
||||
|
||||
epsf=factor*epsmch;
|
||||
epslog=(LM_REAL)log10(eps);
|
||||
|
||||
for(i=0; i<n; ++i)
|
||||
err[i]=zero;
|
||||
|
||||
for(j=0; j<m; ++j){
|
||||
temp=FABS(p[j]);
|
||||
if(temp==zero) temp=one;
|
||||
|
||||
for(i=0; i<n; ++i)
|
||||
err[i]+=temp*fjac[i*m+j];
|
||||
}
|
||||
|
||||
for(i=0; i<n; ++i){
|
||||
temp=one;
|
||||
if(fvec[i]!=zero && fvecp[i]!=zero && FABS(fvecp[i]-fvec[i])>=epsf*FABS(fvec[i]))
|
||||
temp=eps*FABS((fvecp[i]-fvec[i])/eps - err[i])/(FABS(fvec[i])+FABS(fvecp[i]));
|
||||
err[i]=one;
|
||||
if(temp>epsmch && temp<eps)
|
||||
err[i]=((LM_REAL)log10(temp) - epslog)/epslog;
|
||||
if(temp>=eps) err[i]=zero;
|
||||
}
|
||||
|
||||
free(buf);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
/*
|
||||
* This function computes the pseudoinverse of a square matrix A
|
||||
* into B using SVD. A and B can coincide
|
||||
*
|
||||
* The function returns 0 in case of error (e.g. A is singular),
|
||||
* the rank of A if successful
|
||||
*
|
||||
* A, B are mxm
|
||||
*
|
||||
*/
|
||||
static int LEVMAR_PSEUDOINVERSE(LM_REAL *A, LM_REAL *B, int m)
|
||||
{
|
||||
LM_REAL *buf=NULL;
|
||||
int buf_sz=0;
|
||||
static LM_REAL eps=LM_CNST(-1.0);
|
||||
|
||||
register int i, j;
|
||||
LM_REAL *a, *u, *s, *vt, *work;
|
||||
int a_sz, u_sz, s_sz, vt_sz, tot_sz;
|
||||
LM_REAL thresh, one_over_denom;
|
||||
int info, rank, worksz, *iwork, iworksz;
|
||||
|
||||
/* calculate required memory size */
|
||||
worksz=5*m; // min worksize for GESVD
|
||||
//worksz=m*(7*m+4); // min worksize for GESDD
|
||||
iworksz=8*m;
|
||||
a_sz=m*m;
|
||||
u_sz=m*m; s_sz=m; vt_sz=m*m;
|
||||
|
||||
tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
|
||||
|
||||
buf_sz=tot_sz;
|
||||
buf=(LM_REAL *)malloc(buf_sz);
|
||||
if(!buf){
|
||||
fprintf(stderr, RCAT("memory allocation in ", LEVMAR_PSEUDOINVERSE) "() failed!\n");
|
||||
return 0; /* error */
|
||||
}
|
||||
|
||||
a=buf;
|
||||
u=a+a_sz;
|
||||
s=u+u_sz;
|
||||
vt=s+s_sz;
|
||||
work=vt+vt_sz;
|
||||
iwork=(int *)(work+worksz);
|
||||
|
||||
/* store A (column major!) into a */
|
||||
for(i=0; i<m; i++)
|
||||
for(j=0; j<m; j++)
|
||||
a[i+j*m]=A[i*m+j];
|
||||
|
||||
/* SVD decomposition of A */
|
||||
GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
|
||||
//GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);
|
||||
|
||||
/* error treatment */
|
||||
if(info!=0){
|
||||
if(info<0){
|
||||
fprintf(stderr, RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", LEVMAR_PSEUDOINVERSE) "()\n", -info);
|
||||
}
|
||||
else{
|
||||
fprintf(stderr, RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", LEVMAR_PSEUDOINVERSE) "() [info=%d]\n", info);
|
||||
}
|
||||
free(buf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if(eps<0.0){
|
||||
LM_REAL aux;
|
||||
|
||||
/* compute machine epsilon */
|
||||
for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
|
||||
;
|
||||
eps*=LM_CNST(2.0);
|
||||
}
|
||||
|
||||
/* compute the pseudoinverse in B */
|
||||
for(i=0; i<a_sz; i++) B[i]=0.0; /* initialize to zero */
|
||||
for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
|
||||
one_over_denom=LM_CNST(1.0)/s[rank];
|
||||
|
||||
for(j=0; j<m; j++)
|
||||
for(i=0; i<m; i++)
|
||||
B[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
|
||||
}
|
||||
|
||||
free(buf);
|
||||
|
||||
return rank;
|
||||
}
|
||||
#else // no LAPACK
|
||||
|
||||
/*
|
||||
* This function computes the inverse of A in B. A and B can coincide
|
||||
*
|
||||
* The function employs LAPACK-free LU decomposition of A to solve m linear
|
||||
* systems A*B_i=I_i, where B_i and I_i are the i-th columns of B and I.
|
||||
*
|
||||
* A and B are mxm
|
||||
*
|
||||
* The function returns 0 in case of error, 1 if successful
|
||||
*
|
||||
*/
|
||||
static int LEVMAR_LUINVERSE(LM_REAL *A, LM_REAL *B, int m)
|
||||
{
|
||||
void *buf=NULL;
|
||||
//int buf_sz=0;
|
||||
|
||||
register int i, j, k, l;
|
||||
int *idx, maxi=-1, idx_sz, a_sz, x_sz, work_sz, tot_sz;
|
||||
LM_REAL *a, *x, *work, max, sum, tmp;
|
||||
|
||||
/* calculate required memory size */
|
||||
idx_sz=m;
|
||||
a_sz=m*m;
|
||||
x_sz=m;
|
||||
work_sz=m;
|
||||
tot_sz=(a_sz + x_sz + work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */
|
||||
|
||||
//buf_sz=tot_sz;
|
||||
buf=(void *)malloc(tot_sz);
|
||||
if(!buf){
|
||||
fprintf(stderr, RCAT("memory allocation in ", LEVMAR_LUINVERSE) "() failed!\n");
|
||||
return 0; /* error */
|
||||
}
|
||||
|
||||
a=buf;
|
||||
x=a+a_sz;
|
||||
work=x+x_sz;
|
||||
idx=(int *)(work+work_sz);
|
||||
|
||||
/* avoid destroying A by copying it to a */
|
||||
for(i=0; i<a_sz; ++i) a[i]=A[i];
|
||||
|
||||
/* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
|
||||
for(i=0; i<m; ++i){
|
||||
max=0.0;
|
||||
for(j=0; j<m; ++j)
|
||||
if((tmp=FABS(a[i*m+j]))>max)
|
||||
max=tmp;
|
||||
if(max==0.0){
|
||||
fprintf(stderr, RCAT("Singular matrix A in ", LEVMAR_LUINVERSE) "()!\n");
|
||||
free(buf);
|
||||
|
||||
return 0;
|
||||
}
|
||||
work[i]=LM_CNST(1.0)/max;
|
||||
}
|
||||
|
||||
for(j=0; j<m; ++j){
|
||||
for(i=0; i<j; ++i){
|
||||
sum=a[i*m+j];
|
||||
for(k=0; k<i; ++k)
|
||||
sum-=a[i*m+k]*a[k*m+j];
|
||||
a[i*m+j]=sum;
|
||||
}
|
||||
max=0.0;
|
||||
for(i=j; i<m; ++i){
|
||||
sum=a[i*m+j];
|
||||
for(k=0; k<j; ++k)
|
||||
sum-=a[i*m+k]*a[k*m+j];
|
||||
a[i*m+j]=sum;
|
||||
if((tmp=work[i]*FABS(sum))>=max){
|
||||
max=tmp;
|
||||
maxi=i;
|
||||
}
|
||||
}
|
||||
if(j!=maxi){
|
||||
for(k=0; k<m; ++k){
|
||||
tmp=a[maxi*m+k];
|
||||
a[maxi*m+k]=a[j*m+k];
|
||||
a[j*m+k]=tmp;
|
||||
}
|
||||
work[maxi]=work[j];
|
||||
}
|
||||
idx[j]=maxi;
|
||||
if(a[j*m+j]==0.0)
|
||||
a[j*m+j]=LM_REAL_EPSILON;
|
||||
if(j!=m-1){
|
||||
tmp=LM_CNST(1.0)/(a[j*m+j]);
|
||||
for(i=j+1; i<m; ++i)
|
||||
a[i*m+j]*=tmp;
|
||||
}
|
||||
}
|
||||
|
||||
/* The decomposition has now replaced a. Solve the m linear systems using
|
||||
* forward and back substitution
|
||||
*/
|
||||
for(l=0; l<m; ++l){
|
||||
for(i=0; i<m; ++i) x[i]=0.0;
|
||||
x[l]=LM_CNST(1.0);
|
||||
|
||||
for(i=k=0; i<m; ++i){
|
||||
j=idx[i];
|
||||
sum=x[j];
|
||||
x[j]=x[i];
|
||||
if(k!=0)
|
||||
for(j=k-1; j<i; ++j)
|
||||
sum-=a[i*m+j]*x[j];
|
||||
else
|
||||
if(sum!=0.0)
|
||||
k=i+1;
|
||||
x[i]=sum;
|
||||
}
|
||||
|
||||
for(i=m-1; i>=0; --i){
|
||||
sum=x[i];
|
||||
for(j=i+1; j<m; ++j)
|
||||
sum-=a[i*m+j]*x[j];
|
||||
x[i]=sum/a[i*m+i];
|
||||
}
|
||||
|
||||
for(i=0; i<m; ++i)
|
||||
B[i*m+l]=x[i];
|
||||
}
|
||||
|
||||
free(buf);
|
||||
|
||||
return 1;
|
||||
}
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
/*
|
||||
* This function computes in C the covariance matrix corresponding to a least
|
||||
* squares fit. JtJ is the approximate Hessian at the solution (i.e. J^T*J, where
|
||||
* J is the Jacobian at the solution), sumsq is the sum of squared residuals
|
||||
* (i.e. goodnes of fit) at the solution, m is the number of parameters (variables)
|
||||
* and n the number of observations. JtJ can coincide with C.
|
||||
*
|
||||
* if JtJ is of full rank, C is computed as sumsq/(n-m)*(JtJ)^-1
|
||||
* otherwise and if LAPACK is available, C=sumsq/(n-r)*(JtJ)^+
|
||||
* where r is JtJ's rank and ^+ denotes the pseudoinverse
|
||||
* The diagonal of C is made up from the estimates of the variances
|
||||
* of the estimated regression coefficients.
|
||||
* See the documentation of routine E04YCF from the NAG fortran lib
|
||||
*
|
||||
* The function returns the rank of JtJ if successful, 0 on error
|
||||
*
|
||||
* A and C are mxm
|
||||
*
|
||||
*/
|
||||
int LEVMAR_COVAR(LM_REAL *JtJ, LM_REAL *C, LM_REAL sumsq, int m, int n)
|
||||
{
|
||||
register int i;
|
||||
int rnk;
|
||||
LM_REAL fact;
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
rnk=LEVMAR_PSEUDOINVERSE(JtJ, C, m);
|
||||
if(!rnk) return 0;
|
||||
#else
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times")
|
||||
#else
|
||||
#warning LAPACK not available, LU will be used for matrix inversion when computing the covariance; this might be unstable at times
|
||||
#endif // _MSC_VER
|
||||
|
||||
rnk=LEVMAR_LUINVERSE(JtJ, C, m);
|
||||
if(!rnk) return 0;
|
||||
|
||||
rnk=m; /* assume full rank */
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
fact=sumsq/(LM_REAL)(n-rnk);
|
||||
for(i=0; i<m*m; ++i)
|
||||
C[i]*=fact;
|
||||
|
||||
return rnk;
|
||||
}
|
||||
|
||||
/* standard deviation of the best-fit parameter i.
|
||||
* covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
|
||||
*
|
||||
* The standard deviation is computed as \sigma_{i} = \sqrt{C_{ii}}
|
||||
*/
|
||||
LM_REAL LEVMAR_STDDEV(LM_REAL *covar, int m, int i)
|
||||
{
|
||||
return (LM_REAL)sqrt(covar[i*m+i]);
|
||||
}
|
||||
|
||||
/* Pearson's correlation coefficient of the best-fit parameters i and j.
|
||||
* covar is the mxm covariance matrix of the best-fit parameters (see also LEVMAR_COVAR()).
|
||||
*
|
||||
* The coefficient is computed as \rho_{ij} = C_{ij} / sqrt(C_{ii} C_{jj})
|
||||
*/
|
||||
LM_REAL LEVMAR_CORCOEF(LM_REAL *covar, int m, int i, int j)
|
||||
{
|
||||
return (LM_REAL)(covar[i*m+j]/sqrt(covar[i*m+i]*covar[j*m+j]));
|
||||
}
|
||||
|
||||
/* coefficient of determination.
|
||||
* see http://en.wikipedia.org/wiki/Coefficient_of_determination
|
||||
*/
|
||||
LM_REAL LEVMAR_R2(void (*func)(LM_REAL *p, LM_REAL *hx, int m, int n, void *adata),
|
||||
LM_REAL *p, LM_REAL *x, int m, int n, void *adata)
|
||||
{
|
||||
register int i;
|
||||
register LM_REAL tmp;
|
||||
LM_REAL SSerr, // sum of squared errors, i.e. residual sum of squares \sum_i (x_i-hx_i)^2
|
||||
SStot, // \sum_i (x_i-xavg)^2
|
||||
*hx, xavg;
|
||||
|
||||
|
||||
if((hx=(LM_REAL *)malloc(n*sizeof(LM_REAL)))==NULL){
|
||||
fprintf(stderr, RCAT("memory allocation request failed in ", LEVMAR_R2) "()\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/* hx=f(p) */
|
||||
(*func)(p, hx, m, n, adata);
|
||||
|
||||
for(i=n, tmp=0.0; i-->0; )
|
||||
tmp+=x[i];
|
||||
xavg=tmp/(LM_REAL)n;
|
||||
|
||||
if(x)
|
||||
for(i=n, SSerr=SStot=0.0; i-->0; ){
|
||||
tmp=x[i]-hx[i];
|
||||
SSerr+=tmp*tmp;
|
||||
|
||||
tmp=x[i]-xavg;
|
||||
SStot+=tmp*tmp;
|
||||
}
|
||||
else /* x==0 */
|
||||
for(i=n, SSerr=SStot=0.0; i-->0; ){
|
||||
tmp=-hx[i];
|
||||
SSerr+=tmp*tmp;
|
||||
|
||||
tmp=-xavg;
|
||||
SStot+=tmp*tmp;
|
||||
}
|
||||
|
||||
free(hx);
|
||||
|
||||
return LM_CNST(1.0) - SSerr/SStot;
|
||||
}
|
||||
|
||||
/* check box constraints for consistency */
|
||||
int LEVMAR_BOX_CHECK(LM_REAL *lb, LM_REAL *ub, int m)
|
||||
{
|
||||
register int i;
|
||||
|
||||
if(!lb || !ub) return 1;
|
||||
|
||||
for(i=0; i<m; ++i)
|
||||
if(lb[i]>ub[i]) return 0;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
#ifdef HAVE_LAPACK
|
||||
|
||||
/* compute the Cholesky decomposition of C in W, s.t. C=W^t W and W is upper triangular */
|
||||
int LEVMAR_CHOLESKY(LM_REAL *C, LM_REAL *W, int m)
|
||||
{
|
||||
register int i, j;
|
||||
int info;
|
||||
|
||||
/* copy weights array C to W so that LAPACK won't destroy it;
|
||||
* C is assumed symmetric, hence no transposition is needed
|
||||
*/
|
||||
for(i=0, j=m*m; i<j; ++i)
|
||||
W[i]=C[i];
|
||||
|
||||
/* Cholesky decomposition */
|
||||
POTF2("L", (int *)&m, W, (int *)&m, (int *)&info);
|
||||
/* error treatment */
|
||||
if(info!=0){
|
||||
if(info<0){
|
||||
fprintf(stderr, "LAPACK error: illegal value for argument %d of dpotf2 in %s\n", -info, LCAT(LEVMAR_CHOLESKY, "()"));
|
||||
}
|
||||
else{
|
||||
fprintf(stderr, "LAPACK error: the leading minor of order %d is not positive definite,\n%s()\n", info,
|
||||
RCAT("and the Cholesky factorization could not be completed in ", LEVMAR_CHOLESKY));
|
||||
}
|
||||
return LM_ERROR;
|
||||
}
|
||||
|
||||
/* the decomposition is in the lower part of W (in column-major order!).
|
||||
* zeroing the upper part makes it lower triangular which is equivalent to
|
||||
* upper triangular in row-major order
|
||||
*/
|
||||
for(i=0; i<m; i++)
|
||||
for(j=i+1; j<m; j++)
|
||||
W[i+j*m]=0.0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif /* HAVE_LAPACK */
|
||||
|
||||
|
||||
/* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.
|
||||
* e can coincide with either x or y; x can be NULL, in which case it is assumed
|
||||
* to be equal to the zero vector.
|
||||
* Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline
|
||||
* stalls and increase instruction-level parallelism; see http://www.abarnett.demon.co.uk/tutorial.html
|
||||
*/
|
||||
|
||||
LM_REAL LEVMAR_L2NRMXMY(LM_REAL *e, LM_REAL *x, LM_REAL *y, int n)
|
||||
{
|
||||
const int blocksize=8, bpwr=3; /* 8=2^3 */
|
||||
register int i;
|
||||
int j1, j2, j3, j4, j5, j6, j7;
|
||||
int blockn;
|
||||
register LM_REAL sum0=0.0, sum1=0.0, sum2=0.0, sum3=0.0;
|
||||
|
||||
/* n may not be divisible by blocksize,
|
||||
* go as near as we can first, then tidy up.
|
||||
*/
|
||||
blockn = (n>>bpwr)<<bpwr; /* (n / blocksize) * blocksize; */
|
||||
|
||||
/* unroll the loop in blocks of `blocksize'; looping downwards gains some more speed */
|
||||
if(x){
|
||||
for(i=blockn-1; i>0; i-=blocksize){
|
||||
e[i ]=x[i ]-y[i ]; sum0+=e[i ]*e[i ];
|
||||
j1=i-1; e[j1]=x[j1]-y[j1]; sum1+=e[j1]*e[j1];
|
||||
j2=i-2; e[j2]=x[j2]-y[j2]; sum2+=e[j2]*e[j2];
|
||||
j3=i-3; e[j3]=x[j3]-y[j3]; sum3+=e[j3]*e[j3];
|
||||
j4=i-4; e[j4]=x[j4]-y[j4]; sum0+=e[j4]*e[j4];
|
||||
j5=i-5; e[j5]=x[j5]-y[j5]; sum1+=e[j5]*e[j5];
|
||||
j6=i-6; e[j6]=x[j6]-y[j6]; sum2+=e[j6]*e[j6];
|
||||
j7=i-7; e[j7]=x[j7]-y[j7]; sum3+=e[j7]*e[j7];
|
||||
}
|
||||
|
||||
/*
|
||||
* There may be some left to do.
|
||||
* This could be done as a simple for() loop,
|
||||
* but a switch is faster (and more interesting)
|
||||
*/
|
||||
|
||||
i=blockn;
|
||||
if(i<n){
|
||||
/* Jump into the case at the place that will allow
|
||||
* us to finish off the appropriate number of items.
|
||||
*/
|
||||
|
||||
switch(n - i){
|
||||
case 7 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
|
||||
case 6 : e[i]=x[i]-y[i]; sum1+=e[i]*e[i]; ++i;
|
||||
case 5 : e[i]=x[i]-y[i]; sum2+=e[i]*e[i]; ++i;
|
||||
case 4 : e[i]=x[i]-y[i]; sum3+=e[i]*e[i]; ++i;
|
||||
case 3 : e[i]=x[i]-y[i]; sum0+=e[i]*e[i]; ++i;
|
||||
case 2 : e[i]=x[i]-y[i]; sum1+=e[i]*e[i]; ++i;
|
||||
case 1 : e[i]=x[i]-y[i]; sum2+=e[i]*e[i]; //++i;
|
||||
}
|
||||
}
|
||||
}
|
||||
else{ /* x==0 */
|
||||
for(i=blockn-1; i>0; i-=blocksize){
|
||||
e[i ]=-y[i ]; sum0+=e[i ]*e[i ];
|
||||
j1=i-1; e[j1]=-y[j1]; sum1+=e[j1]*e[j1];
|
||||
j2=i-2; e[j2]=-y[j2]; sum2+=e[j2]*e[j2];
|
||||
j3=i-3; e[j3]=-y[j3]; sum3+=e[j3]*e[j3];
|
||||
j4=i-4; e[j4]=-y[j4]; sum0+=e[j4]*e[j4];
|
||||
j5=i-5; e[j5]=-y[j5]; sum1+=e[j5]*e[j5];
|
||||
j6=i-6; e[j6]=-y[j6]; sum2+=e[j6]*e[j6];
|
||||
j7=i-7; e[j7]=-y[j7]; sum3+=e[j7]*e[j7];
|
||||
}
|
||||
|
||||
/*
|
||||
* There may be some left to do.
|
||||
* This could be done as a simple for() loop,
|
||||
* but a switch is faster (and more interesting)
|
||||
*/
|
||||
|
||||
i=blockn;
|
||||
if(i<n){
|
||||
/* Jump into the case at the place that will allow
|
||||
* us to finish off the appropriate number of items.
|
||||
*/
|
||||
|
||||
switch(n - i){
|
||||
case 7 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
|
||||
case 6 : e[i]=-y[i]; sum1+=e[i]*e[i]; ++i;
|
||||
case 5 : e[i]=-y[i]; sum2+=e[i]*e[i]; ++i;
|
||||
case 4 : e[i]=-y[i]; sum3+=e[i]*e[i]; ++i;
|
||||
case 3 : e[i]=-y[i]; sum0+=e[i]*e[i]; ++i;
|
||||
case 2 : e[i]=-y[i]; sum1+=e[i]*e[i]; ++i;
|
||||
case 1 : e[i]=-y[i]; sum2+=e[i]*e[i]; //++i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return sum0+sum1+sum2+sum3;
|
||||
}
|
||||
|
||||
/* undefine everything. THIS MUST REMAIN AT THE END OF THE FILE */
|
||||
#undef POTF2
|
||||
#undef GESVD
|
||||
#undef GESDD
|
||||
#undef GEMM
|
||||
#undef LEVMAR_PSEUDOINVERSE
|
||||
#undef LEVMAR_LUINVERSE
|
||||
#undef LEVMAR_BOX_CHECK
|
||||
#undef LEVMAR_CHOLESKY
|
||||
#undef LEVMAR_COVAR
|
||||
#undef LEVMAR_STDDEV
|
||||
#undef LEVMAR_CORCOEF
|
||||
#undef LEVMAR_R2
|
||||
#undef LEVMAR_CHKJAC
|
||||
#undef LEVMAR_FDIF_FORW_JAC_APPROX
|
||||
#undef LEVMAR_FDIF_CENT_JAC_APPROX
|
||||
#undef LEVMAR_TRANS_MAT_MAT_MULT
|
||||
#undef LEVMAR_L2NRMXMY
|
||||
Reference in New Issue
Block a user