QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2007, 2008, 2009, 2010, 2011, 2012. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 
00025 
00026 #include <qvmath/qvnumericalanalysis.h>
00027 
00028 const QVVector qvEstimateGradient(QVFunction<QVVector, double> &multivariateFunction, const QVVector &location, const double h)
00029     {
00030     const int dim = location.size();
00031     QVVector gradient(dim);
00032     const double actual = multivariateFunction(location);
00033     for (int i = 0; i < dim; i++)
00034         {
00035         QVVector stepLocation = location;
00036         stepLocation[i] += h;
00037         gradient[i] = (multivariateFunction(stepLocation) - actual)/h;
00038         }
00039     return gradient;
00040     }
00041 
00042 const QVMatrix qvEstimateJacobian(QVFunction<QVVector, QVVector> &multivariateFunction, const QVVector &location, const double h)
00043     {
00044     const QVVector actual = multivariateFunction(location);
00045 
00046     QVMatrix jacobian(actual.size(), location.size());
00047 
00048     for (int i = 0; i < location.size(); i++)
00049         {
00050         QVVector stepLocation = location;
00051         stepLocation[i] += h;
00052         jacobian.setCol(i, (multivariateFunction(stepLocation) - actual)/h);
00053         }
00054 
00055     return jacobian;
00056     }
00057 
00058 const QVMatrix qvEstimateHessian(       QVFunction<QVVector, double> &multivariateFunction,
00059                     const QVVector &location, const double h)
00060     {
00061     const int dim = location.size();
00062     const QVVector g = qvEstimateGradient(multivariateFunction, location, h);
00063 
00064     QVMatrix hessian(dim, dim);
00065 
00066     const double actual = multivariateFunction(location);
00067     for (int i = 0; i < dim; i++)
00068         for (int j = 0; j < dim; j++)
00069         {
00070         QVVector stepLocationIJ = location;
00071         stepLocationIJ[i] += h;
00072         stepLocationIJ[j] += h;
00073         hessian(i,j) = (multivariateFunction(stepLocationIJ) - actual)/(h*h) - (g[i] + g[j])/h;
00074         }
00075     return hessian;
00076     }
00077 
00078 #ifdef GSL_AVAILABLE
00079 
00080 // GSL minimization
00081 
00082 double my_f (const gsl_vector *v, void *params)
00083     {
00084     return ((QVFunction<QVVector, double> *) params)->operator()(QVVector(v));
00085     }
00086 
00087 /* The gradient of f, df = (df/dx, df/dy). */
00088 void my_df (const gsl_vector *v, void *params, gsl_vector *df)
00089     {
00090     gsl_vector_memcpy(df, qvEstimateGradient( * (QVFunction<QVVector, double> *) params,QVVector(v)));
00091     }
00092 
00093 /* Compute both f and df together. */
00094 void my_fdf (const gsl_vector *x, void *params, double *f, gsl_vector *df)
00095     {
00096     *f = my_f(x, params);
00097     my_df(x, params, df);
00098     }
00099 
00100 bool qvGSLMinimizeFDF ( const QVFunction<QVVector, double> & function, QVVector &point,
00101                 const GSLMultiminFDFMinimizerType gslMinimizerAlgorithm,
00102                 const int maxIterations, const double maxGradientNorm,
00103                 const double step, const double tol)
00104     {
00105     const int dims = point.size();
00106     const gsl_multimin_fdfminimizer_type *minimizer_type = NULL;
00107     switch(gslMinimizerAlgorithm)
00108         {
00109         case ConjugateFR:       minimizer_type = gsl_multimin_fdfminimizer_conjugate_fr;        break;
00110         case ConjugatePR:       minimizer_type = gsl_multimin_fdfminimizer_conjugate_pr;        break;
00111         case VectorBFGS:        minimizer_type = gsl_multimin_fdfminimizer_vector_bfgs;         break;
00112         case SteepestDescent:   minimizer_type = gsl_multimin_fdfminimizer_steepest_descent;    break;
00113         }
00114 
00115     gsl_multimin_fdfminimizer *minimizer = gsl_multimin_fdfminimizer_alloc (minimizer_type, dims);
00116 
00117     gsl_multimin_function_fdf my_func;
00118     my_func.n = dims;
00119     my_func.f = &my_f;
00120     my_func.df = &my_df;
00121     my_func.fdf = &my_fdf;
00122     my_func.params = const_cast<QVFunction<QVVector, double> *>(&function);
00123 
00124     gsl_vector *x = point;
00125 
00126     gsl_multimin_fdfminimizer_set (minimizer, &my_func, x, step, tol);
00127 
00128     int status = GSL_CONTINUE;
00129     for (int i = 0; status == GSL_CONTINUE && i < maxIterations; i++)
00130         {
00131         if ((status = gsl_multimin_fdfminimizer_iterate (minimizer)))
00132             break;
00133 
00134         status = gsl_multimin_test_gradient (gsl_multimin_fdfminimizer_gradient(minimizer), maxGradientNorm);
00135         }
00136 
00137     // Store resulting value.
00138     point = QVVector(gsl_multimin_fdfminimizer_x(minimizer));
00139 
00140     gsl_multimin_fdfminimizer_free (minimizer);
00141 
00142     gsl_vector_free (x);
00143 
00144     return (status == GSL_SUCCESS);
00145     }
00146 
00148 
00149 double my_f_gradient (const gsl_vector *v, void *params)
00150     {
00151     return ((QPair< QVFunction<QVVector, double> *, QVFunction<QVVector, QVVector> *> *) params)->first->operator()(QVVector(v));
00152     }
00153 
00154 /* The gradient of f, df = (df/dx, df/dy). */
00155 void my_df_gradient (const gsl_vector *v, void *params, gsl_vector *df)
00156     {
00157     gsl_vector_memcpy(df, ((QPair< QVFunction<QVVector, double> *, QVFunction<QVVector, QVVector> *> *) params)->second->operator()(QVVector(v)));
00158     }
00159 
00160 /* Compute both f and df together. */
00161 void my_fdf_gradient (const gsl_vector *x, void *params, double *f, gsl_vector *df)
00162     {
00163     *f = my_f_gradient(x, params);
00164     my_df_gradient(x, params, df);
00165     }
00166 
00167 bool qvGSLMinimizeFDF ( const QVFunction<QVVector, double> & function, const QVFunction<QVVector, QVVector> & gradientFunction,
00168                 QVVector &point, const GSLMultiminFDFMinimizerType gslMinimizerAlgorithm,
00169                 const int maxIterations, const double maxGradientNorm,
00170                 const double step, const double tol)
00171     {
00172     const int dims = point.size();
00173     const gsl_multimin_fdfminimizer_type *minimizer_type = NULL;
00174     switch(gslMinimizerAlgorithm)
00175         {
00176         case ConjugateFR:       minimizer_type = gsl_multimin_fdfminimizer_conjugate_fr;        break;
00177         case ConjugatePR:       minimizer_type = gsl_multimin_fdfminimizer_conjugate_pr;        break;
00178         case VectorBFGS:        minimizer_type = gsl_multimin_fdfminimizer_vector_bfgs;         break;
00179         case SteepestDescent:   minimizer_type = gsl_multimin_fdfminimizer_steepest_descent;    break;
00180         }
00181 
00182     gsl_multimin_fdfminimizer *minimizer = gsl_multimin_fdfminimizer_alloc (minimizer_type, dims);
00183     QPair< const QVFunction<QVVector, double> *, const QVFunction<QVVector, QVVector> *> functions(&function, &gradientFunction);
00184 
00185     gsl_multimin_function_fdf my_func;
00186     my_func.n = dims;
00187     my_func.f = &my_f_gradient;
00188     my_func.df = &my_df_gradient;
00189     my_func.fdf = &my_fdf_gradient;
00190     my_func.params = &functions;
00191 
00192     gsl_vector *x = point;
00193 
00194     gsl_multimin_fdfminimizer_set (minimizer, &my_func, x, step, tol);
00195 
00196     int status = GSL_CONTINUE;
00197     for (int i = 0; status == GSL_CONTINUE && i < maxIterations; i++)
00198         {
00199         if ((status = gsl_multimin_fdfminimizer_iterate (minimizer)))
00200             break;
00201 
00202         status = gsl_multimin_test_gradient (gsl_multimin_fdfminimizer_gradient(minimizer), maxGradientNorm);
00203         }
00204 
00205     // Store resulting value.
00206     point = QVVector(gsl_multimin_fdfminimizer_x(minimizer));
00207 
00208     gsl_multimin_fdfminimizer_free (minimizer);
00209 
00210     gsl_vector_free (x);
00211 
00212     return (status == GSL_SUCCESS);
00213     }
00214 
00215 int multifit_f (const gsl_vector * x, void *params, gsl_vector * f)
00216     {
00217     const QVVector result = ((QPair< QVFunction<QVVector, QVVector> *, QVFunction<QVVector, QVMatrix> *> *)params)->first->operator()(QVVector(x));
00218 
00219     gsl_vector_memcpy(f, result);
00220 
00221     return GSL_SUCCESS;
00222     }
00223 
00224 int multifit_df (const gsl_vector * x, void *params, gsl_matrix * J)
00225     {
00226     const QVMatrix result = ((QPair< QVFunction<QVVector, QVVector> *, QVFunction<QVVector, QVMatrix> *> *)params)->second->operator()(QVVector(x));
00227 
00228     gsl_matrix_memcpy(J, result);
00229 
00230     return GSL_SUCCESS;
00231     }
00232 
00233 int multifit_fdf (const gsl_vector * x, void *params, gsl_vector * f, gsl_matrix * J)
00234     {
00235     multifit_f (x, params, f);
00236     multifit_df (x, params, J);
00237 
00238     return GSL_SUCCESS;
00239     }
00240 
00241 bool qvGSLSolveFDF (    const QVFunction<QVVector, QVVector> & function, QVFunction<QVVector, QVMatrix> & functionJacobian,
00242                 QVVector &x, const GSLMultiminFDFSolverType gslSolverAlgorithm,
00243                 const int maxIterations, const double maxAbsErr, const double maxRelErr)
00244     {
00245     const gsl_multifit_fdfsolver_type *solver_type = NULL;
00246     switch(gslSolverAlgorithm)
00247         {
00248         case LMScaledDerivative:        solver_type = gsl_multifit_fdfsolver_lmsder;    break;
00249         case LMDerivative:              solver_type = gsl_multifit_fdfsolver_lmder;     break;
00250         }
00251 
00252     gsl_vector *x_gsl = x;
00253 
00254     QPair< const QVFunction<QVVector, QVVector> *, const QVFunction<QVVector, QVMatrix> *> functions(&function, &functionJacobian);
00255 
00256     // Just to get the input and output vectors sizes, for the function.
00257     const QVMatrix      jacobian = functionJacobian(x);
00258     const int   inputVectorSize = jacobian.getCols(),   // Size of input vector for function.
00259             outputVectorSize = jacobian.getRows();      // Size of output vector for function.
00260 
00261     // Generate minimization problem.
00262     gsl_multifit_function_fdf f;
00263     f.f = &multifit_f;
00264     f.df = &multifit_df;
00265     f.fdf = &multifit_fdf;
00266     f.n = outputVectorSize;
00267     f.p = inputVectorSize;
00268     f.params = &functions;
00269 
00270     gsl_multifit_fdfsolver *s = gsl_multifit_fdfsolver_alloc (solver_type, outputVectorSize, inputVectorSize);
00271     gsl_multifit_fdfsolver_set (s, &f, x_gsl);
00272 
00273     // Perform minimization.
00274     int iter = 0, status;
00275     do  {
00276         iter++;
00277         status = gsl_multifit_fdfsolver_iterate (s);
00278 
00279         if (status)
00280             break;
00281 
00282         status = gsl_multifit_test_delta (s->dx, s->x, maxAbsErr, maxRelErr);
00283         }
00284     while (status == GSL_CONTINUE && iter < maxIterations);
00285 
00286     // Print output data.
00287     x = s->x;
00288 
00289     gsl_multifit_fdfsolver_free (s);
00290     gsl_vector_free (x_gsl);
00291 
00292     return (status == GSL_SUCCESS);
00293     }
00294 
00296 
00297 double fn2 (double x, void * params)
00298     {
00299     return ((QVFunction<double, double> *) params)->operator()(x);
00300     }
00301 
00302 bool qvGSLMinimize(const QVFunction<double, double> &function,
00303                          double &x, double &lower, double &upper,
00304                          const GSLMinFMinimizer gslMinimizerAlgorithm,
00305                          const int maxIterations,
00306                          const double absoluteError,
00307                          const double relativeError)
00308     {
00309     const gsl_min_fminimizer_type *minimizer_type =
00310             (gslMinimizerAlgorithm == GoldenSection)? gsl_min_fminimizer_goldensection : gsl_min_fminimizer_brent;
00311 
00312     gsl_function F;
00313     F.function = &fn2;
00314     F.params = (void *) &function;
00315 
00316     gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (minimizer_type);
00317     gsl_min_fminimizer_set (s, &F, x, lower, upper);
00318 
00319     for (int i = 0; i <maxIterations; i++)
00320         {
00321         gsl_min_fminimizer_iterate(s);
00322         x = gsl_min_fminimizer_x_minimum (s);
00323         lower = gsl_min_fminimizer_x_lower (s);
00324         upper = gsl_min_fminimizer_x_upper (s);
00325         if (gsl_min_test_interval(lower, upper, absoluteError, relativeError) != GSL_CONTINUE)
00326             break;
00327         }
00328 
00329     gsl_min_fminimizer_free (s);
00330 
00331     return gsl_min_test_interval(lower, upper, absoluteError, relativeError) == GSL_SUCCESS;
00332     }
00333 #endif
src/qvmath/qvnumericalanalysis.cpp
