| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2013, OpenCV Foundation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of the copyright holders may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the OpenCV Foundation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "precomp.hpp" |
| |
| #if 0 |
| #define dprintf(x) printf x |
| #define print_matrix(x) print(x) |
| #else |
| #define dprintf(x) |
| #define print_matrix(x) |
| #endif |
| |
| /* |
| |
| ****Error Message******************************************************************************************************************** |
| |
| Downhill Simplex method in OpenCV dev 3.0.0 getting this error: |
| |
| OpenCV Error: Assertion failed (dims <= 2 && data && (unsigned)i0 < (unsigned)(s ize.p[0] * size.p[1]) |
| && elemSize() == (((((DataType<_Tp>::type) & ((512 - 1) << 3)) >> 3) + 1) << ((((sizeof(size_t)/4+1)16384|0x3a50) |
| >> ((DataType<_Tp>::typ e) & ((1 << 3) - 1))2) & 3))) in Mat::at, |
| file C:\builds\master_PackSlave-w in32-vc12-shared\opencv\modules\core\include\opencv2/core/mat.inl.hpp, line 893 |
| |
| ****Problem and Possible Fix********************************************************************************************************* |
| |
| DownhillSolverImpl::innerDownhillSimplex something looks broken here: |
| Mat_<double> coord_sum(1,ndim,0.0),buf(1,ndim,0.0),y(1,ndim,0.0); |
| fcount = 0; |
| for(i=0;i<ndim+1;++i) |
| { |
| y(i) = f->calc(p[i]); |
| } |
| |
| y has only ndim elements, while the loop goes over ndim+1 |
| |
| Edited the following for possible fix: |
| |
| Replaced y(1,ndim,0.0) ------> y(1,ndim+1,0.0) |
| |
| *********************************************************************************************************************************** |
| |
| The code below was used in tesing the source code. |
| Created by @SareeAlnaghy |
| |
| #include <iostream> |
| #include <cstdlib> |
| #include <cmath> |
| #include <algorithm> |
| #include <opencv2\optim\optim.hpp> |
| |
| using namespace std; |
| using namespace cv; |
| |
| void test(Ptr<optim::DownhillSolver> MinProblemSolver, Ptr<optim::MinProblemSolver::Function> ptr_F, Mat &P, Mat &step) |
| { |
| try{ |
| |
| MinProblemSolver->setFunction(ptr_F); |
| MinProblemSolver->setInitStep(step); |
| double res = MinProblemSolver->minimize(P); |
| |
| cout << "res " << res << endl; |
| } |
| catch (exception e) |
| { |
| cerr << "Error:: " << e.what() << endl; |
| } |
| } |
| |
| int main() |
| { |
| |
| class DistanceToLines :public optim::MinProblemSolver::Function { |
| public: |
| double calc(const double* x)const{ |
| |
| return x[0] * x[0] + x[1] * x[1]; |
| |
| } |
| }; |
| |
| Mat P = (Mat_<double>(1, 2) << 1.0, 1.0); |
| Mat step = (Mat_<double>(2, 1) << -0.5, 0.5); |
| |
| Ptr<optim::MinProblemSolver::Function> ptr_F(new DistanceToLines()); |
| Ptr<optim::DownhillSolver> MinProblemSolver = optim::createDownhillSolver(); |
| |
| test(MinProblemSolver, ptr_F, P, step); |
| |
| system("pause"); |
| return 0; |
| } |
| |
| ****Suggesttion for imporving Simplex implentation*************************************************************************************** |
| |
| Currently the downhilll simplex method outputs the function value that is minimized. It should also return the coordinate points where the |
| function is minimized. This is very useful in many applications such as using back projection methods to find a point of intersection of |
| multiple lines in three dimensions as not all lines intersect in three dimensions. |
| |
| */ |
| |
| namespace cv |
| { |
| |
| class DownhillSolverImpl : public DownhillSolver |
| { |
| public: |
| DownhillSolverImpl() |
| { |
| _Function=Ptr<Function>(); |
| _step=Mat_<double>(); |
| } |
| |
| void getInitStep(OutputArray step) const { _step.copyTo(step); } |
| void setInitStep(InputArray step) |
| { |
| // set dimensionality and make a deep copy of step |
| Mat m = step.getMat(); |
| dprintf(("m.cols=%d\nm.rows=%d\n", m.cols, m.rows)); |
| if( m.rows == 1 ) |
| m.copyTo(_step); |
| else |
| transpose(m, _step); |
| } |
| |
| Ptr<MinProblemSolver::Function> getFunction() const { return _Function; } |
| |
| void setFunction(const Ptr<Function>& f) { _Function=f; } |
| |
| TermCriteria getTermCriteria() const { return _termcrit; } |
| |
| void setTermCriteria( const TermCriteria& termcrit ) |
| { |
| CV_Assert( termcrit.type == (TermCriteria::MAX_ITER + TermCriteria::EPS) && |
| termcrit.epsilon > 0 && |
| termcrit.maxCount > 0 ); |
| _termcrit=termcrit; |
| } |
| |
| double minimize( InputOutputArray x_ ) |
| { |
| dprintf(("hi from minimize\n")); |
| CV_Assert( !_Function.empty() ); |
| CV_Assert( std::min(_step.cols, _step.rows) == 1 && |
| std::max(_step.cols, _step.rows) >= 2 && |
| _step.type() == CV_64FC1 ); |
| dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon)); |
| dprintf(("step\n")); |
| print_matrix(_step); |
| |
| Mat x = x_.getMat(), simplex; |
| |
| createInitialSimplex(x, simplex, _step); |
| int count = 0; |
| double res = innerDownhillSimplex(simplex,_termcrit.epsilon, _termcrit.epsilon, |
| count, _termcrit.maxCount); |
| dprintf(("%d iterations done\n",count)); |
| |
| if( !x.empty() ) |
| { |
| Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>()); |
| simplex_0m.convertTo(x, x.type()); |
| } |
| else |
| { |
| int x_type = x_.fixedType() ? x_.type() : CV_64F; |
| simplex.row(0).convertTo(x_, x_type); |
| } |
| return res; |
| } |
| protected: |
| Ptr<MinProblemSolver::Function> _Function; |
| TermCriteria _termcrit; |
| Mat _step; |
| |
| inline void updateCoordSum(const Mat& p, Mat& coord_sum) |
| { |
| int i, j, m = p.rows, n = p.cols; |
| double* coord_sum_ = coord_sum.ptr<double>(); |
| CV_Assert( coord_sum.cols == n && coord_sum.rows == 1 ); |
| |
| for( j = 0; j < n; j++ ) |
| coord_sum_[j] = 0.; |
| |
| for( i = 0; i < m; i++ ) |
| { |
| const double* p_i = p.ptr<double>(i); |
| for( j = 0; j < n; j++ ) |
| coord_sum_[j] += p_i[j]; |
| } |
| |
| dprintf(("\nupdated coord sum:\n")); |
| print_matrix(coord_sum); |
| |
| } |
| |
| inline void createInitialSimplex( const Mat& x0, Mat& simplex, Mat& step ) |
| { |
| int i, j, ndim = step.cols; |
| CV_Assert( _Function->getDims() == ndim ); |
| Mat x = x0; |
| if( x0.empty() ) |
| x = Mat::zeros(1, ndim, CV_64F); |
| CV_Assert( (x.cols == 1 && x.rows == ndim) || (x.cols == ndim && x.rows == 1) ); |
| CV_Assert( x.type() == CV_32F || x.type() == CV_64F ); |
| |
| simplex.create(ndim + 1, ndim, CV_64F); |
| Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>()); |
| |
| x.convertTo(simplex_0m, CV_64F); |
| double* simplex_0 = simplex.ptr<double>(); |
| const double* step_ = step.ptr<double>(); |
| for( i = 1; i <= ndim; i++ ) |
| { |
| double* simplex_i = simplex.ptr<double>(i); |
| for( j = 0; j < ndim; j++ ) |
| simplex_i[j] = simplex_0[j]; |
| simplex_i[i-1] += 0.5*step_[i-1]; |
| } |
| for( j = 0; j < ndim; j++ ) |
| simplex_0[j] -= 0.5*step_[j]; |
| |
| dprintf(("\nthis is simplex\n")); |
| print_matrix(simplex); |
| } |
| |
| /* |
| Performs the actual minimization of MinProblemSolver::Function f (after the initialization was done) |
| |
| The matrix p[ndim+1][1..ndim] represents ndim+1 vertices that |
| form a simplex - each row is an ndim vector. |
| On output, fcount gives the number of function evaluations taken. |
| */ |
| double innerDownhillSimplex( Mat& p, double MinRange, double MinError, int& fcount, int nmax ) |
| { |
| int i, j, ndim = p.cols; |
| Mat coord_sum(1, ndim, CV_64F), buf(1, ndim, CV_64F), y(1, ndim+1, CV_64F); |
| double* y_ = y.ptr<double>(); |
| |
| fcount = ndim+1; |
| for( i = 0; i <= ndim; i++ ) |
| y_[i] = calc_f(p.ptr<double>(i)); |
| |
| updateCoordSum(p, coord_sum); |
| |
| for (;;) |
| { |
| // find highest (worst), next-to-worst, and lowest |
| // (best) points by going through all of them. |
| int ilo = 0, ihi, inhi; |
| if( y_[0] > y_[1] ) |
| { |
| ihi = 0; inhi = 1; |
| } |
| else |
| { |
| ihi = 1; inhi = 0; |
| } |
| for( i = 0; i <= ndim; i++ ) |
| { |
| double yval = y_[i]; |
| if (yval <= y_[ilo]) |
| ilo = i; |
| if (yval > y_[ihi]) |
| { |
| inhi = ihi; |
| ihi = i; |
| } |
| else if (yval > y_[inhi] && i != ihi) |
| inhi = i; |
| } |
| CV_Assert( ihi != inhi ); |
| if( ilo == inhi || ilo == ihi ) |
| { |
| for( i = 0; i <= ndim; i++ ) |
| { |
| double yval = y_[i]; |
| if( yval == y_[ilo] && i != ihi && i != inhi ) |
| { |
| ilo = i; |
| break; |
| } |
| } |
| } |
| dprintf(("\nthis is y on iteration %d:\n",fcount)); |
| print_matrix(y); |
| |
| // check stop criterion |
| double error = fabs(y_[ihi] - y_[ilo]); |
| double range = 0; |
| for( j = 0; j < ndim; j++ ) |
| { |
| double minval, maxval; |
| minval = maxval = p.at<double>(0, j); |
| for( i = 1; i <= ndim; i++ ) |
| { |
| double pval = p.at<double>(i, j); |
| minval = std::min(minval, pval); |
| maxval = std::max(maxval, pval); |
| } |
| range = std::max(range, fabs(maxval - minval)); |
| } |
| |
| if( range <= MinRange || error <= MinError || fcount >= nmax ) |
| { |
| // Put best point and value in first slot. |
| std::swap(y_[0], y_[ilo]); |
| for( j = 0; j < ndim; j++ ) |
| { |
| std::swap(p.at<double>(0, j), p.at<double>(ilo, j)); |
| } |
| break; |
| } |
| |
| double y_lo = y_[ilo], y_nhi = y_[inhi], y_hi = y_[ihi]; |
| // Begin a new iteration. First, reflect the worst point about the centroid of others |
| double alpha = -1.0; |
| double y_alpha = tryNewPoint(p, coord_sum, ihi, alpha, buf, fcount); |
| |
| dprintf(("\ny_lo=%g, y_nhi=%g, y_hi=%g, y_alpha=%g, p_alpha:\n", y_lo, y_nhi, y_hi, y_alpha)); |
| print_matrix(buf); |
| |
| if( y_alpha < y_nhi ) |
| { |
| if( y_alpha < y_lo ) |
| { |
| // If that's better than the best point, go twice as far in that direction |
| double beta = -2.0; |
| double y_beta = tryNewPoint(p, coord_sum, ihi, beta, buf, fcount); |
| dprintf(("\ny_beta=%g, p_beta:\n", y_beta)); |
| print_matrix(buf); |
| if( y_beta < y_alpha ) |
| { |
| alpha = beta; |
| y_alpha = y_beta; |
| } |
| } |
| replacePoint(p, coord_sum, y, ihi, alpha, y_alpha); |
| } |
| else |
| { |
| // The new point is worse than the second-highest, |
| // do not go so far in that direction |
| double gamma = 0.5; |
| double y_gamma = tryNewPoint(p, coord_sum, ihi, gamma, buf, fcount); |
| dprintf(("\ny_gamma=%g, p_gamma:\n", y_gamma)); |
| print_matrix(buf); |
| if( y_gamma < y_hi ) |
| replacePoint(p, coord_sum, y, ihi, gamma, y_gamma); |
| else |
| { |
| // Can't seem to improve things. |
| // Contract the simplex to good point |
| // in hope to find a simplex landscape. |
| for( i = 0; i <= ndim; i++ ) |
| { |
| if (i != ilo) |
| { |
| for( j = 0; j < ndim; j++ ) |
| p.at<double>(i, j) = 0.5*(p.at<double>(i, j) + p.at<double>(ilo, j)); |
| y_[i] = calc_f(p.ptr<double>(i)); |
| } |
| } |
| fcount += ndim; |
| updateCoordSum(p, coord_sum); |
| } |
| } |
| dprintf(("\nthis is simplex on iteration %d\n",fcount)); |
| print_matrix(p); |
| } |
| return y_[0]; |
| } |
| |
| inline double calc_f(const double* ptr) |
| { |
| double res = _Function->calc(ptr); |
| CV_Assert( !cvIsNaN(res) && !cvIsInf(res) ); |
| return res; |
| } |
| |
| double tryNewPoint( Mat& p, Mat& coord_sum, int ihi, double alpha_, Mat& ptry, int& fcount ) |
| { |
| int j, ndim = p.cols; |
| |
| double alpha = (1.0 - alpha_)/ndim; |
| double beta = alpha - alpha_; |
| double* p_ihi = p.ptr<double>(ihi); |
| double* ptry_ = ptry.ptr<double>(); |
| double* coord_sum_ = coord_sum.ptr<double>(); |
| |
| for( j = 0; j < ndim; j++ ) |
| ptry_[j] = coord_sum_[j]*alpha - p_ihi[j]*beta; |
| |
| fcount++; |
| return calc_f(ptry_); |
| } |
| |
| void replacePoint( Mat& p, Mat& coord_sum, Mat& y, int ihi, double alpha_, double ytry ) |
| { |
| int j, ndim = p.cols; |
| |
| double alpha = (1.0 - alpha_)/ndim; |
| double beta = alpha - alpha_; |
| double* p_ihi = p.ptr<double>(ihi); |
| double* coord_sum_ = coord_sum.ptr<double>(); |
| |
| for( j = 0; j < ndim; j++ ) |
| p_ihi[j] = coord_sum_[j]*alpha - p_ihi[j]*beta; |
| y.at<double>(ihi) = ytry; |
| updateCoordSum(p, coord_sum); |
| } |
| }; |
| |
| |
| // both minRange & minError are specified by termcrit.epsilon; |
| // In addition, user may specify the number of iterations that the algorithm does. |
| Ptr<DownhillSolver> DownhillSolver::create( const Ptr<MinProblemSolver::Function>& f, |
| InputArray initStep, TermCriteria termcrit ) |
| { |
| Ptr<DownhillSolver> DS = makePtr<DownhillSolverImpl>(); |
| DS->setFunction(f); |
| DS->setInitStep(initStep); |
| DS->setTermCriteria(termcrit); |
| return DS; |
| } |
| |
| } |
