downhill_simplex.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                           License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of the copyright holders may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the OpenCV Foundation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include "precomp.hpp"
00042 
00043 #if 0
00044 #define dprintf(x) printf x
00045 #define print_matrix(x) print(x)
00046 #else
00047 #define dprintf(x)
00048 #define print_matrix(x)
00049 #endif
00050 
00051 /*
00052 
00053 ****Error Message********************************************************************************************************************
00054 
00055 Downhill Simplex method in OpenCV dev 3.0.0 getting this error:
00056 
00057 OpenCV Error: Assertion failed (dims <= 2 && data && (unsigned)i0 < (unsigned)(s ize.p[0] * size.p[1])
00058 && elemSize() == (((((DataType<_Tp>::type) & ((512 - 1) << 3)) >> 3) + 1) << ((((sizeof(size_t)/4+1)16384|0x3a50)
00059 >> ((DataType<_Tp>::typ e) & ((1 << 3) - 1))2) & 3))) in Mat::at,
00060 file C:\builds\master_PackSlave-w in32-vc12-shared\opencv\modules\core\include\opencv2/core/mat.inl.hpp, line 893
00061 
00062 ****Problem and Possible Fix*********************************************************************************************************
00063 
00064 DownhillSolverImpl::innerDownhillSimplex something looks broken here:
00065 Mat_<double> coord_sum(1,ndim,0.0),buf(1,ndim,0.0),y(1,ndim,0.0);
00066 fcount = 0;
00067 for(i=0;i<ndim+1;++i)
00068 {
00069 y(i) = f->calc(p[i]);
00070 }
00071 
00072 y has only ndim elements, while the loop goes over ndim+1
00073 
00074 Edited the following for possible fix:
00075 
00076 Replaced y(1,ndim,0.0) ------> y(1,ndim+1,0.0)
00077 
00078 ***********************************************************************************************************************************
00079 
00080 The code below was used in tesing the source code.
00081 Created by @SareeAlnaghy
00082 
00083 #include <iostream>
00084 #include <cstdlib>
00085 #include <cmath>
00086 #include <algorithm>
00087 #include <opencv2\optim\optim.hpp>
00088 
00089 using namespace std;
00090 using namespace cv;
00091 
00092 void test(Ptr<optim::DownhillSolver> MinProblemSolver, Ptr<optim::MinProblemSolver::Function> ptr_F, Mat &P, Mat &step)
00093 {
00094 try{
00095 
00096 MinProblemSolver->setFunction(ptr_F);
00097 MinProblemSolver->setInitStep(step);
00098 double res = MinProblemSolver->minimize(P);
00099 
00100 cout << "res " << res << endl;
00101 }
00102 catch (exception e)
00103 {
00104 cerr << "Error:: " << e.what() << endl;
00105 }
00106 }
00107 
00108 int main()
00109 {
00110 
00111 class DistanceToLines :public optim::MinProblemSolver::Function {
00112 public:
00113 double calc(const double* x)const{
00114 
00115 return x[0] * x[0] + x[1] * x[1];
00116 
00117 }
00118 };
00119 
00120 Mat P = (Mat_<double>(1, 2) << 1.0, 1.0);
00121 Mat step = (Mat_<double>(2, 1) << -0.5, 0.5);
00122 
00123 Ptr<optim::MinProblemSolver::Function> ptr_F(new DistanceToLines());
00124 Ptr<optim::DownhillSolver> MinProblemSolver = optim::createDownhillSolver();
00125 
00126 test(MinProblemSolver, ptr_F, P, step);
00127 
00128 system("pause");
00129 return 0;
00130 }
00131 
00132 ****Suggesttion for imporving Simplex implentation***************************************************************************************
00133 
00134 Currently the downhilll simplex method outputs the function value that is minimized. It should also return the coordinate points where the
00135 function is minimized. This is very useful in many applications such as using back projection methods to find a point of intersection of
00136 multiple lines in three dimensions as not all lines intersect in three dimensions.
00137 
00138 */
00139 
00140 namespace cv
00141 {
00142 
00143 class DownhillSolverImpl : public DownhillSolver
00144 {
00145 public:
00146     DownhillSolverImpl()
00147     {
00148         _Function=Ptr<Function>();
00149         _step=Mat_<double>();
00150     }
00151 
00152     void getInitStep(OutputArray step) const { _step.copyTo(step); }
00153     void setInitStep(InputArray step)
00154     {
00155         // set dimensionality and make a deep copy of step
00156         Mat m = step.getMat();
00157         dprintf(("m.cols=%d\nm.rows=%d\n", m.cols, m.rows));
00158         if( m.rows == 1 )
00159             m.copyTo(_step);
00160         else
00161             transpose(m, _step);
00162     }
00163 
00164     Ptr<MinProblemSolver::Function> getFunction() const { return _Function; }
00165 
00166     void setFunction(const Ptr<Function>& f) { _Function=f; }
00167 
00168     TermCriteria getTermCriteria() const { return _termcrit; }
00169 
00170     void setTermCriteria( const TermCriteria& termcrit )
00171     {
00172         CV_Assert( termcrit.type == (TermCriteria::MAX_ITER + TermCriteria::EPS) &&
00173                    termcrit.epsilon > 0 &&
00174                    termcrit.maxCount > 0 );
00175         _termcrit=termcrit;
00176     }
00177 
00178     double minimize( InputOutputArray x_ )
00179     {
00180         dprintf(("hi from minimize\n"));
00181         CV_Assert( !_Function.empty() );
00182         CV_Assert( std::min(_step.cols, _step.rows) == 1 &&
00183                   std::max(_step.cols, _step.rows) >= 2 &&
00184                   _step.type() == CV_64FC1 );
00185         dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon));
00186         dprintf(("step\n"));
00187         print_matrix(_step);
00188 
00189         Mat x = x_.getMat(), simplex;
00190 
00191         createInitialSimplex(x, simplex, _step);
00192         int count = 0;
00193         double res = innerDownhillSimplex(simplex,_termcrit.epsilon, _termcrit.epsilon,
00194                                           count, _termcrit.maxCount);
00195         dprintf(("%d iterations done\n",count));
00196 
00197         if( !x.empty() )
00198         {
00199             Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>());
00200             simplex_0m.convertTo(x, x.type());
00201         }
00202         else
00203         {
00204             int x_type = x_.fixedType() ? x_.type() : CV_64F;
00205             simplex.row(0).convertTo(x_, x_type);
00206         }
00207         return res;
00208     }
00209 protected:
00210     Ptr<MinProblemSolver::Function> _Function;
00211     TermCriteria _termcrit;
00212     Mat _step;
00213 
00214     inline void updateCoordSum(const Mat& p, Mat& coord_sum)
00215     {
00216         int i, j, m = p.rows, n = p.cols;
00217         double* coord_sum_ = coord_sum.ptr<double>();
00218         CV_Assert( coord_sum.cols == n && coord_sum.rows == 1 );
00219 
00220         for( j = 0; j < n; j++ )
00221             coord_sum_[j] = 0.;
00222 
00223         for( i = 0; i < m; i++ )
00224         {
00225             const double* p_i = p.ptr<double>(i);
00226             for( j = 0; j < n; j++ )
00227                 coord_sum_[j] += p_i[j];
00228         }
00229 
00230         dprintf(("\nupdated coord sum:\n"));
00231         print_matrix(coord_sum);
00232 
00233     }
00234 
00235     inline void createInitialSimplex( const Mat& x0, Mat& simplex, Mat& step )
00236     {
00237         int i, j, ndim = step.cols;
00238         CV_Assert( _Function->getDims() == ndim );
00239         Mat x = x0;
00240         if( x0.empty() )
00241             x = Mat::zeros(1, ndim, CV_64F);
00242         CV_Assert( (x.cols == 1 && x.rows == ndim) || (x.cols == ndim && x.rows == 1) );
00243         CV_Assert( x.type() == CV_32F || x.type() == CV_64F );
00244 
00245         simplex.create(ndim + 1, ndim, CV_64F);
00246         Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>());
00247 
00248         x.convertTo(simplex_0m, CV_64F);
00249         double* simplex_0 = simplex.ptr<double>();
00250         const double* step_ = step.ptr<double>();
00251         for( i = 1; i <= ndim; i++ )
00252         {
00253             double* simplex_i = simplex.ptr<double>(i);
00254             for( j = 0; j < ndim; j++ )
00255                 simplex_i[j] = simplex_0[j];
00256             simplex_i[i-1] += 0.5*step_[i-1];
00257         }
00258         for( j = 0; j < ndim; j++ )
00259             simplex_0[j] -= 0.5*step_[j];
00260 
00261         dprintf(("\nthis is simplex\n"));
00262         print_matrix(simplex);
00263     }
00264 
00265     /*
00266      Performs the actual minimization of MinProblemSolver::Function f (after the initialization was done)
00267 
00268      The matrix p[ndim+1][1..ndim] represents ndim+1 vertices that
00269      form a simplex - each row is an ndim vector.
00270      On output, fcount gives the number of function evaluations taken.
00271     */
00272     double innerDownhillSimplex( Mat& p, double MinRange, double MinError, int& fcount, int nmax )
00273     {
00274         int i, j, ndim = p.cols;
00275         Mat coord_sum(1, ndim, CV_64F), buf(1, ndim, CV_64F), y(1, ndim+1, CV_64F);
00276         double* y_ = y.ptr<double>();
00277 
00278         fcount = ndim+1;
00279         for( i = 0; i <= ndim; i++ )
00280             y_[i] = calc_f(p.ptr<double>(i));
00281 
00282         updateCoordSum(p, coord_sum);
00283 
00284         for (;;)
00285         {
00286             // find highest (worst), next-to-worst, and lowest
00287             // (best) points by going through all of them.
00288             int ilo = 0, ihi, inhi;
00289             if( y_[0] > y_[1] )
00290             {
00291                 ihi = 0; inhi = 1;
00292             }
00293             else
00294             {
00295                 ihi = 1; inhi = 0;
00296             }
00297             for( i = 0; i <= ndim; i++ )
00298             {
00299                 double yval = y_[i];
00300                 if (yval <= y_[ilo])
00301                     ilo = i;
00302                 if (yval > y_[ihi])
00303                 {
00304                     inhi = ihi;
00305                     ihi = i;
00306                 }
00307                 else if (yval > y_[inhi] && i != ihi)
00308                     inhi = i;
00309             }
00310             CV_Assert( ihi != inhi );
00311             if( ilo == inhi || ilo == ihi )
00312             {
00313                 for( i = 0; i <= ndim; i++ )
00314                 {
00315                     double yval = y_[i];
00316                     if( yval == y_[ilo] && i != ihi && i != inhi )
00317                     {
00318                         ilo = i;
00319                         break;
00320                     }
00321                 }
00322             }
00323             dprintf(("\nthis is y on iteration %d:\n",fcount));
00324             print_matrix(y);
00325 
00326             // check stop criterion
00327             double error = fabs(y_[ihi] - y_[ilo]);
00328             double range = 0;
00329             for( j = 0; j < ndim; j++ )
00330             {
00331                 double minval, maxval;
00332                 minval = maxval = p.at<double>(0, j);
00333                 for( i = 1; i <= ndim; i++ )
00334                 {
00335                     double pval = p.at<double>(i, j);
00336                     minval = std::min(minval, pval);
00337                     maxval = std::max(maxval, pval);
00338                 }
00339                 range = std::max(range, fabs(maxval - minval));
00340             }
00341 
00342             if( range <= MinRange || error <= MinError || fcount >= nmax )
00343             {
00344                 // Put best point and value in first slot.
00345                 std::swap(y_[0], y_[ilo]);
00346                 for( j = 0; j < ndim; j++ )
00347                 {
00348                     std::swap(p.at<double>(0, j), p.at<double>(ilo, j));
00349                 }
00350                 break;
00351             }
00352 
00353             double y_lo = y_[ilo], y_nhi = y_[inhi], y_hi = y_[ihi];
00354             // Begin a new iteration. First, reflect the worst point about the centroid of others
00355             double alpha = -1.0;
00356             double y_alpha = tryNewPoint(p, coord_sum, ihi, alpha, buf, fcount);
00357 
00358             dprintf(("\ny_lo=%g, y_nhi=%g, y_hi=%g, y_alpha=%g, p_alpha:\n", y_lo, y_nhi, y_hi, y_alpha));
00359             print_matrix(buf);
00360 
00361             if( y_alpha < y_nhi )
00362             {
00363                 if( y_alpha < y_lo )
00364                 {
00365                     // If that's better than the best point, go twice as far in that direction
00366                     double beta = -2.0;
00367                     double y_beta = tryNewPoint(p, coord_sum, ihi, beta, buf, fcount);
00368                     dprintf(("\ny_beta=%g, p_beta:\n", y_beta));
00369                     print_matrix(buf);
00370                     if( y_beta < y_alpha )
00371                     {
00372                         alpha = beta;
00373                         y_alpha = y_beta;
00374                     }
00375                 }
00376                 replacePoint(p, coord_sum, y, ihi, alpha, y_alpha);
00377             }
00378             else
00379             {
00380                 // The new point is worse than the second-highest,
00381                 // do not go so far in that direction
00382                 double gamma = 0.5;
00383                 double y_gamma = tryNewPoint(p, coord_sum, ihi, gamma, buf, fcount);
00384                 dprintf(("\ny_gamma=%g, p_gamma:\n", y_gamma));
00385                 print_matrix(buf);
00386                 if( y_gamma < y_hi )
00387                     replacePoint(p, coord_sum, y, ihi, gamma, y_gamma);
00388                 else
00389                 {
00390                     // Can't seem to improve things.
00391                     // Contract the simplex to good point
00392                     // in hope to find a simplex landscape.
00393                     for( i = 0; i <= ndim; i++ )
00394                     {
00395                         if (i != ilo)
00396                         {
00397                             for( j = 0; j < ndim; j++ )
00398                                 p.at<double>(i, j) = 0.5*(p.at<double>(i, j) + p.at<double>(ilo, j));
00399                             y_[i] = calc_f(p.ptr<double>(i));
00400                         }
00401                     }
00402                     fcount += ndim;
00403                     updateCoordSum(p, coord_sum);
00404                 }
00405             }
00406             dprintf(("\nthis is simplex on iteration %d\n",fcount));
00407             print_matrix(p);
00408         }
00409         return y_[0];
00410     }
00411 
00412     inline double calc_f(const double* ptr)
00413     {
00414         double res = _Function->calc(ptr);
00415         CV_Assert( !cvIsNaN(res) && !cvIsInf(res) );
00416         return res;
00417     }
00418 
00419     double tryNewPoint( Mat& p, Mat& coord_sum, int ihi, double alpha_, Mat& ptry, int& fcount )
00420     {
00421         int j, ndim = p.cols;
00422 
00423         double alpha = (1.0 - alpha_)/ndim;
00424         double beta = alpha - alpha_;
00425         double* p_ihi = p.ptr<double>(ihi);
00426         double* ptry_ = ptry.ptr<double>();
00427         double* coord_sum_ = coord_sum.ptr<double>();
00428 
00429         for( j = 0; j < ndim; j++ )
00430             ptry_[j] = coord_sum_[j]*alpha - p_ihi[j]*beta;
00431 
00432         fcount++;
00433         return calc_f(ptry_);
00434     }
00435 
00436     void replacePoint( Mat& p, Mat& coord_sum, Mat& y, int ihi, double alpha_, double ytry )
00437     {
00438         int j, ndim = p.cols;
00439 
00440         double alpha = (1.0 - alpha_)/ndim;
00441         double beta = alpha - alpha_;
00442         double* p_ihi = p.ptr<double>(ihi);
00443         double* coord_sum_ = coord_sum.ptr<double>();
00444 
00445         for( j = 0; j < ndim; j++ )
00446             p_ihi[j] = coord_sum_[j]*alpha - p_ihi[j]*beta;
00447         y.at<double>(ihi) = ytry;
00448         updateCoordSum(p, coord_sum);
00449     }
00450 };
00451 
00452 
00453 // both minRange & minError are specified by termcrit.epsilon;
00454 // In addition, user may specify the number of iterations that the algorithm does.
00455 Ptr<DownhillSolver> DownhillSolver::create( const Ptr<MinProblemSolver::Function> & f,
00456                                             InputArray initStep, TermCriteria termcrit )
00457 {
00458     Ptr<DownhillSolver> DS = makePtr<DownhillSolverImpl>();
00459     DS->setFunction(f);
00460     DS->setInitStep(initStep);
00461     DS->setTermCriteria(termcrit);
00462     return DS;
00463 }
00464 
00465 }
00466