28 #include <Base/Common/CompareFloatingPoint.hh>
29 #include <MathAlgo/minpack/cminpack.h>
35 long int Powell(minpack_func_nn fun,
void *p,
41 long int n = InitialGuess.
Size();
42 long int lwa = (n*(3*n+13))/2;
43 double *wa =
new double[lwa];
44 Result = InitialGuess;
46 long int ret = (
long int)hybrd1(fun, p,n, Result.
GetData(), FuncVec.
GetData(),
53 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
58 double EpsilonFun,
double Tolerance)
62 long int n = InitialGuess.
Size();
63 double *wa =
new double[4*n];
64 Result = InitialGuess;
66 long int maxfev = 200*(n + 1);
70 long int lr = (n*(n + 1))/2;
73 double *diag =
new double[n];
74 for (
long int i = 0; i < n; i++) diag[i] = 1.0;
77 double *fjac =
new double[n*n];
79 double *r =
new double[lr];
80 double *qtf =
new double[n];
81 long int ret = (
long int)hybrd(fun, p, n, Result.
GetData(), FuncVec.
GetData(), Tolerance,
82 maxfev, ml, mu, EpsilonFun, diag, mode, factor, nprint,
83 &nfev, fjac, ldfjac, r, lr, qtf,
84 &wa[0], &wa[n], &wa[2*n], &wa[3*n]);
94 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
103 BIASERR(
"Target function is underdetermined (" << m <<
" residuals < "
104 << n <<
" parameters!");
110 Result = InitialGuess;
112 double *fjac =
new double[m*n];
115 int *ipvt =
new int[n];
116 long int lwa = 5*n+m;
117 double *wa=
new double[lwa];
118 long int ret = (
long int)lmder1(fun, p, m, n, Result.
GetData(), FuncVec.
GetData(), fjac,
119 ldfjac, Tolerance, ipvt, wa, lwa);
127 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
133 long int m,
long int n,
141 BIASERR(
"Target function is underdetermined (" << m <<
" residuals < "
142 << n <<
" parameters!");
148 Result = InitialGuess;
150 long int maxfev = (MaxIter == LM_DEF_MAX_ITER) ? 200*(n + 1) : MaxIter;
153 double ftol = Tolerance;
154 double xtol = Tolerance;
158 double *diag =
new double[n];
160 int *iptv =
new int[n];
161 double *qtf =
new double[n];
162 double *wa =
new double[m+3*n];
165 double *fjac =
new double[m*n];
167 long int ret = (
long int)lmder(fun, p, m, n, Result.
GetData(),
168 FuncVec.
GetData(), fjac, ldfjac,
169 ftol, xtol, gtol, maxfev, diag,
170 mode, factor, nprint, &nfev, &njfev, iptv, qtf,
171 &wa[0], &wa[n], &wa[2*n], &wa[3*n]);
181 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
190 BIASERR(
"Target function is underdetermined (" << m <<
" residuals < "
191 << n <<
" parameters!");
193 if (MaxIter != LM_DEF_MAX_ITER) {
194 BIASWARN(
"Parameter MaxIter is not used and should be removed here!");
200 Result = InitialGuess;
203 int *iwa =
new int[n];
204 long int lwa = m*n+5*n+m;
205 double *wa =
new double[lwa];
206 long int ret = (
long int)lmdif1(fun, p, m, n, Result.
GetData(), FuncVec.
GetData(),
207 Tolerance, iwa, wa, lwa);
214 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
229 double *SumOfSquaredErrors)
232 if (num_errors < num_vars) {
233 BIASERR(
"Target function is underdetermined (" << num_errors
234 <<
" residuals < " << num_vars <<
" parameters!");
241 Result = InitialGuess;
243 long int maxfev = (MaxIter == LM_DEF_MAX_ITER) ? 200*(num_vars + 1) : MaxIter;
246 double ftol = Tolerance;
247 double xtol = Tolerance;
248 double gtol = Tolerance;
251 double *diag =
new double[num_vars];
253 const bool ExhaustiveData =
254 (fjac != NULL) && (ipvt != NULL) && (SumOfSquaredErrors != NULL);
255 if (!ExhaustiveData) {
256 fjac =
new double[num_vars*num_errors];
257 ipvt =
new int[num_vars];
259 const long int ldfjac = num_errors;
260 double *qtf =
new double[num_vars];
261 double *wa =
new double[num_errors+3*num_vars];
264 ret = (
long int)lmdif(fun, p, num_errors, num_vars, Result.
GetData(),
266 gtol, maxfev, EpsilonFun, diag, mode, factor, nprint,
267 &nfev, fjac, ldfjac, ipvt, qtf, &wa[0], &wa[num_vars],
268 &wa[2*num_vars], &wa[3*num_vars]);
272 if (!ExhaustiveData) {
277 *SumOfSquaredErrors = 0;
278 for (
long int i = 0; i < num_vars; i++)
279 *SumOfSquaredErrors += qtf[i]*qtf[i];
285 return (ret > 0 && ret < 5) ? 0 : (ret == 0 ? -1 : ret);
290 int *Permutation,
double &SumOfSquaredErrors,
297 memcpy(mJac.
GetData(), Jac, NumErrors*NumParams*
sizeof(double));
298 memcpy(mperm.
GetData(), Permutation, NumParams*
sizeof(int));
300 SumOfSquaredErrors, Cov);
307 const double &SumOfSquaredErrors,
313 Cov.
newsize(NumParams, NumParams);
335 for (row = 0; row < NumParams; row++)
336 for (column = 0; column < NumParams; column++)
337 Cov[row][column] = (column<row) ? 0.0 : Jac[column][row];
341 for (row = 0; row < NumParams; row++) {
343 Cov[row][row] = (
Equal(Cov[row][row], 0.0)) ? 0.0 : 1.0 / Cov[row][row];
345 for (column = row + 1; column < NumParams; column++) {
347 for (k = row; k < column; k++) {
348 s -= Cov[row][k] * Cov[k][column];
351 (
Equal(Cov[row][column], 0.0)) ? 0.0 : s / Cov[column][column];
356 for (row = 0; row < NumParams; row++) {
357 for (column = 0; column < NumParams; column++) {
358 TMP[row][column] = 0;
359 for (k = 0; k < NumParams; k++) {
360 TMP[row][column] += Cov[row][k] * Cov[column][k];
367 NumParams * NumParams *
sizeof(double) );
371 for (row = 0; row < NumParams; row++) {
372 P[Permutation[row]-1][row] = 1;
377 for (row = 0; row < NumParams; row++) {
378 for (column = 0; column < NumParams; column++) {
379 TMP[row][column] = 0;
380 for (k = 0; k < NumParams; k++) {
381 TMP[row][column] += Cov[row][k] * P[column][k];
387 double sigma0squared;
388 if (NumErrors > NumParams) {
389 sigma0squared = SumOfSquaredErrors / (double)(NumErrors - NumParams);
391 BIASERR(
"Covariance approximation is not valid, NumErrors = NumParams!");
392 sigma0squared = SumOfSquaredErrors;
412 long int m = NumErrors;
413 long int n = (
long int)x.
size();
428 fun(p, m, n, mx.
GetData(), fvec.GetData(), fjac.
GetData(), ldfjac, iflag);
431 fun(p, m, n, xp.GetData(), fvecp.
GetData(), fjac.
GetData(), ldfjac,iflag);
435 fun(p, m, n, mx.
GetData(), fvec.GetData(), fjac.
GetData(), ldfjac, iflag);
439 chkder(m, n, mx.
GetData(), fvec.GetData(), fjac.
GetData(), ldfjac,
446 const double gradientEpsilon)
450 long int m = NumErrors;
451 long int n = (
long int)x.
size();
458 double eps = gradientEpsilon;
459 double *wa =
new double[m];
465 mfjac.
GetData(), ldfjac, eps, wa);
470 return (ret == 0 ? 0 : -1);
477 const unsigned int m = A.
GetRows();
478 const unsigned int n = A.
GetCols();
487 double *A_Data =
new double[m*n];
493 for(
unsigned int j=0; j<n; j++){
494 for(
unsigned int i=0; i<m; i++){
495 A_Data[j*m + i] = A[i][j];
502 double *rdiag =
new double[n];
504 double *acnorm =
new double[n];
506 double *wa =
new double[n];
507 qrfac(m, n, A_Data, lda, pivot, ipvt, lipvt, rdiag, acnorm, wa);
514 for(
unsigned int j = 0; j < n; j++){
515 for(
unsigned int i = 0; i <= j && i < m; i++){
519 R[i][j] = A_Data[j*m+i];
529 for (
unsigned int j = 0; j < n && j < m; j++) {
531 for (
unsigned int i = j; i < m; i++) {
532 vec[i] = A_Data[j*m+i];
long int LevenbergMarquardtExtended(minpack_funcder_mn fun, void *p, long int m, long int n, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance, long int MaxIter)
Solves an overdetermined system f(x) = 0 of m non-linear equations with n parameters using the extend...
Matrix< T > Transpose() const
transpose function, storing data destination matrix
Matrix< T > & newsize(Subscript M, Subscript N)
unsigned int Size() const
length of the vector
long int ComputeCovariance(long int NumErrors, long int NumParams, double *Jac, int *Permutation, double &SumOfSquaredErrors, Matrix< double > &Cov)
Compute covariance matrix from Levenberg-Marquardt resulting Jacobian matrix J(x) and permutation mat...
void QRFrac(const Matrix< double > &A, Matrix< double > &Q, Matrix< double > &R)
Compute the QR decomposition of input matrix A using Householder transformations without column pivot...
void SetZero()
equivalent to matrix call
#define TMP
Test Matrix I/O precision issues. Deprecated!
Vector< T > & newsize(Subscript N)
Matrix< T > OuterProduct(const Vector< T > &v) const
outer product, constructs a matrix.
void SetZero()
Sets all values to zero.
unsigned int GetRows() const
void CheckJacobian(minpack_funcder_mn fun, void *p, const long int NumErrors, const Vector< double > &x, Vector< double > &error)
Convenience function to check correctness of user supplied Jacobians J(x) needed by the analytical ve...
long int ComputeJacobian(minpack_func_mn fun, void *p, const int NumErrors, const Vector< double > &x, Matrix< double > &fjac, const double gradientEpsilon)
Compute the Jacobian J(x) of function f at x.
long int LevenbergMarquardt(minpack_funcder_mn fun, void *p, long int m, long int n, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solves an overdetermined system f(x) = 0 of m non-linear equations with n parameters using the Levenb...
T * GetData()
get the pointer to the data array of the matrix (for faster direct memeory access) ...
void SetIdentity()
Converts matrix to identity matrix.
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
unsigned int GetCols() const
T * GetData() const
get the pointer to the data array of the vector (for faster direct memory access) ...
void Mult(const Matrix< T > &arg, Matrix< T > &result) const
matrix multiplication, result is not allocated
void MultiplyIP(const T &scalar)
in place multiplication function
long int Powell(minpack_func_nn fun, void *p, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solve a system f(x) = 0 of n non-linear equations with n unknowns using the Powell hybrid method...
long int PowellExtended(minpack_func_nn fun, void *p, Vector< double > &InitialGuess, Vector< double > &Result, double EpsilonFun, double Tolerance)
Solve a system f(n) = 0 of n non-linear equations with n unknowns using the Powell hybrid method...