|
|
|
|
@ -40,8 +40,13 @@ |
|
|
|
|
//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 |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
|
|
|
|
|
@ -51,14 +56,14 @@ 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 cv::Mat::at, |
|
|
|
|
>> ((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); |
|
|
|
|
nfunk = 0; |
|
|
|
|
fcount = 0; |
|
|
|
|
for(i=0;i<ndim+1;++i) |
|
|
|
|
{ |
|
|
|
|
y(i) = f->calc(p[i]); |
|
|
|
|
@ -135,275 +140,326 @@ multiple lines in three dimensions as not all lines intersect in three dimension |
|
|
|
|
namespace cv |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
class DownhillSolverImpl : public DownhillSolver |
|
|
|
|
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 ) |
|
|
|
|
{ |
|
|
|
|
public: |
|
|
|
|
void getInitStep(OutputArray step) const; |
|
|
|
|
void setInitStep(InputArray step); |
|
|
|
|
Ptr<Function> getFunction() const; |
|
|
|
|
void setFunction(const Ptr<Function>& f); |
|
|
|
|
TermCriteria getTermCriteria() const; |
|
|
|
|
DownhillSolverImpl(); |
|
|
|
|
void setTermCriteria(const TermCriteria& termcrit); |
|
|
|
|
double minimize(InputOutputArray x); |
|
|
|
|
protected: |
|
|
|
|
Ptr<MinProblemSolver::Function> _Function; |
|
|
|
|
TermCriteria _termcrit; |
|
|
|
|
Mat _step; |
|
|
|
|
Mat_<double> buf_x; |
|
|
|
|
|
|
|
|
|
private: |
|
|
|
|
inline void createInitialSimplex(Mat_<double>& simplex,Mat& step); |
|
|
|
|
inline double innerDownhillSimplex(cv::Mat_<double>& p,double MinRange,double MinError,int& nfunk, |
|
|
|
|
const Ptr<MinProblemSolver::Function>& f,int nmax); |
|
|
|
|
inline double tryNewPoint(Mat_<double>& p,Mat_<double>& y,Mat_<double>& coord_sum,const Ptr<MinProblemSolver::Function>& f,int ihi, |
|
|
|
|
double fac,Mat_<double>& ptry); |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
double DownhillSolverImpl::tryNewPoint( |
|
|
|
|
Mat_<double>& p, |
|
|
|
|
Mat_<double>& y, |
|
|
|
|
Mat_<double>& coord_sum, |
|
|
|
|
const Ptr<MinProblemSolver::Function>& f, |
|
|
|
|
int ihi, |
|
|
|
|
double fac, |
|
|
|
|
Mat_<double>& ptry |
|
|
|
|
) |
|
|
|
|
CV_Assert( termcrit.type == (TermCriteria::MAX_ITER + TermCriteria::EPS) && |
|
|
|
|
termcrit.epsilon > 0 && |
|
|
|
|
termcrit.maxCount > 0 ); |
|
|
|
|
_termcrit=termcrit; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
double minimize( InputOutputArray x_ ) |
|
|
|
|
{ |
|
|
|
|
int ndim=p.cols; |
|
|
|
|
int j; |
|
|
|
|
double fac1,fac2,ytry; |
|
|
|
|
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)); |
|
|
|
|
|
|
|
|
|
fac1=(1.0-fac)/ndim; |
|
|
|
|
fac2=fac1-fac; |
|
|
|
|
for (j=0;j<ndim;j++) |
|
|
|
|
if( !x.empty() ) |
|
|
|
|
{ |
|
|
|
|
ptry(j)=coord_sum(j)*fac1-p(ihi,j)*fac2; |
|
|
|
|
Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>()); |
|
|
|
|
simplex_0m.convertTo(x, x.type()); |
|
|
|
|
} |
|
|
|
|
ytry=f->calc(ptry.ptr<double>()); |
|
|
|
|
if (ytry < y(ihi)) |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
y(ihi)=ytry; |
|
|
|
|
for (j=0;j<ndim;j++) |
|
|
|
|
{ |
|
|
|
|
coord_sum(j) += ptry(j)-p(ihi,j); |
|
|
|
|
p(ihi,j)=ptry(j); |
|
|
|
|
} |
|
|
|
|
int x_type = x_.fixedType() ? x_.type() : CV_64F; |
|
|
|
|
simplex.row(0).convertTo(x_, x_type); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return ytry; |
|
|
|
|
return res; |
|
|
|
|
} |
|
|
|
|
protected: |
|
|
|
|
Ptr<MinProblemSolver::Function> _Function; |
|
|
|
|
TermCriteria _termcrit; |
|
|
|
|
Mat _step; |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
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, nfunk gives the number of function evaluations taken. |
|
|
|
|
*/ |
|
|
|
|
double DownhillSolverImpl::innerDownhillSimplex( |
|
|
|
|
cv::Mat_<double>& p, |
|
|
|
|
double MinRange, |
|
|
|
|
double MinError, |
|
|
|
|
int& nfunk, |
|
|
|
|
const Ptr<MinProblemSolver::Function>& f, |
|
|
|
|
int nmax |
|
|
|
|
) |
|
|
|
|
inline void updateCoordSum(const Mat& p, Mat& coord_sum) |
|
|
|
|
{ |
|
|
|
|
int ndim=p.cols; |
|
|
|
|
double res; |
|
|
|
|
int i,ihi,ilo,inhi,j,mpts=ndim+1; |
|
|
|
|
double error, range,ysave,ytry; |
|
|
|
|
Mat_<double> coord_sum(1,ndim,0.0),buf(1,ndim,0.0),y(1,ndim+1,0.0); |
|
|
|
|
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 ); |
|
|
|
|
|
|
|
|
|
nfunk = 0; |
|
|
|
|
for( j = 0; j < n; j++ ) |
|
|
|
|
coord_sum_[j] = 0.; |
|
|
|
|
|
|
|
|
|
for(i=0;i<ndim+1;++i) |
|
|
|
|
for( i = 0; i < m; i++ ) |
|
|
|
|
{ |
|
|
|
|
y(i) = f->calc(p[i]); |
|
|
|
|
const double* p_i = p.ptr<double>(i); |
|
|
|
|
for( j = 0; j < n; j++ ) |
|
|
|
|
coord_sum_[j] += p_i[j]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
nfunk = ndim+1; |
|
|
|
|
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)); |
|
|
|
|
|
|
|
|
|
reduce(p,coord_sum,0,CV_REDUCE_SUM); |
|
|
|
|
updateCoordSum(p, coord_sum); |
|
|
|
|
|
|
|
|
|
for (;;) |
|
|
|
|
{ |
|
|
|
|
ilo=0; |
|
|
|
|
/* find highest (worst), next-to-worst, and lowest
|
|
|
|
|
(best) points by going through all of them. */ |
|
|
|
|
ihi = y(0)>y(1) ? (inhi=1,0) : (inhi=0,1); |
|
|
|
|
for (i=0;i<mpts;i++) |
|
|
|
|
// 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 ) |
|
|
|
|
{ |
|
|
|
|
if (y(i) <= y(ilo)) |
|
|
|
|
ilo=i; |
|
|
|
|
if (y(i) > y(ihi)) |
|
|
|
|
for( i = 0; i <= ndim; i++ ) |
|
|
|
|
{ |
|
|
|
|
inhi=ihi; |
|
|
|
|
ihi=i; |
|
|
|
|
double yval = y_[i]; |
|
|
|
|
if( yval == y_[ilo] && i != ihi && i != inhi ) |
|
|
|
|
{ |
|
|
|
|
ilo = i; |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
else if (y(i) > y(inhi) && i != ihi) |
|
|
|
|
inhi=i; |
|
|
|
|
} |
|
|
|
|
dprintf(("\nthis is y on iteration %d:\n",fcount)); |
|
|
|
|
print_matrix(y); |
|
|
|
|
|
|
|
|
|
/* check stop criterion */ |
|
|
|
|
error=fabs(y(ihi)-y(ilo)); |
|
|
|
|
range=0; |
|
|
|
|
for(i=0;i<ndim;++i) |
|
|
|
|
// check stop criterion
|
|
|
|
|
double error = fabs(y_[ihi] - y_[ilo]); |
|
|
|
|
double range = 0; |
|
|
|
|
for( j = 0; j < ndim; j++ ) |
|
|
|
|
{ |
|
|
|
|
double min = p(0,i); |
|
|
|
|
double max = p(0,i); |
|
|
|
|
double d; |
|
|
|
|
for(j=1;j<=ndim;++j) |
|
|
|
|
double minval, maxval; |
|
|
|
|
minval = maxval = p.at<double>(0, j); |
|
|
|
|
for( i = 1; i <= ndim; i++ ) |
|
|
|
|
{ |
|
|
|
|
if( min > p(j,i) ) min = p(j,i); |
|
|
|
|
if( max < p(j,i) ) max = p(j,i); |
|
|
|
|
double pval = p.at<double>(i, j); |
|
|
|
|
minval = std::min(minval, pval); |
|
|
|
|
maxval = std::max(maxval, pval); |
|
|
|
|
} |
|
|
|
|
d = fabs(max-min); |
|
|
|
|
if(range < d) range = d; |
|
|
|
|
range = std::max(range, fabs(maxval - minval)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if(range <= MinRange || error <= MinError) |
|
|
|
|
{ /* Put best point and value in first slot. */ |
|
|
|
|
std::swap(y(0),y(ilo)); |
|
|
|
|
for (i=0;i<ndim;i++) |
|
|
|
|
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(0,i),p(ilo,i)); |
|
|
|
|
std::swap(p.at<double>(0, j), p.at<double>(ilo, j)); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (nfunk >= nmax){ |
|
|
|
|
dprintf(("nmax exceeded\n")); |
|
|
|
|
return y(ilo); |
|
|
|
|
} |
|
|
|
|
nfunk += 2; |
|
|
|
|
/*Begin a new iteration. First, reflect the worst point about the centroid of others */ |
|
|
|
|
ytry = tryNewPoint(p,y,coord_sum,f,ihi,-1.0,buf); |
|
|
|
|
if (ytry <= y(ilo)) |
|
|
|
|
{ /*If that's better than the best point, go twice as far in that direction*/ |
|
|
|
|
ytry = tryNewPoint(p,y,coord_sum,f,ihi,2.0,buf); |
|
|
|
|
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 if (ytry >= y(inhi)) |
|
|
|
|
{ /* The new point is worse than the second-highest, but better
|
|
|
|
|
than the worst so do not go so far in that direction */ |
|
|
|
|
ysave = y(ihi); |
|
|
|
|
ytry = tryNewPoint(p,y,coord_sum,f,ihi,0.5,buf); |
|
|
|
|
if (ytry >= ysave) |
|
|
|
|
{ /* Can't seem to improve things. Contract the simplex to good point
|
|
|
|
|
in hope to find a simplex landscape. */ |
|
|
|
|
for (i=0;i<mpts;i++) |
|
|
|
|
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(i,j) = coord_sum(j) = 0.5*(p(i,j)+p(ilo,j)); |
|
|
|
|
} |
|
|
|
|
y(i)=f->calc(coord_sum.ptr<double>()); |
|
|
|
|
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)); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
nfunk += ndim; |
|
|
|
|
reduce(p,coord_sum,0,CV_REDUCE_SUM); |
|
|
|
|
fcount += ndim; |
|
|
|
|
updateCoordSum(p, coord_sum); |
|
|
|
|
} |
|
|
|
|
} else --(nfunk); /* correct nfunk */ |
|
|
|
|
dprintf(("this is simplex on iteration %d\n",nfunk)); |
|
|
|
|
} |
|
|
|
|
dprintf(("\nthis is simplex on iteration %d\n",fcount)); |
|
|
|
|
print_matrix(p); |
|
|
|
|
} /* go to next iteration. */ |
|
|
|
|
res = y(0); |
|
|
|
|
} |
|
|
|
|
return y_[0]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
inline double calc_f(const double* ptr) |
|
|
|
|
{ |
|
|
|
|
double res = _Function->calc(ptr); |
|
|
|
|
CV_Assert( !cvIsNaN(res) && !cvIsInf(res) ); |
|
|
|
|
return res; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void DownhillSolverImpl::createInitialSimplex(Mat_<double>& simplex,Mat& step){ |
|
|
|
|
for(int i=1;i<=step.cols;++i) |
|
|
|
|
{ |
|
|
|
|
simplex.row(0).copyTo(simplex.row(i)); |
|
|
|
|
simplex(i,i-1)+= 0.5*step.at<double>(0,i-1); |
|
|
|
|
} |
|
|
|
|
simplex.row(0) -= 0.5*step; |
|
|
|
|
double tryNewPoint( Mat& p, Mat& coord_sum, int ihi, double alpha_, Mat& ptry, int& fcount ) |
|
|
|
|
{ |
|
|
|
|
int j, ndim = p.cols; |
|
|
|
|
|
|
|
|
|
dprintf(("this is simplex\n")); |
|
|
|
|
print_matrix(simplex); |
|
|
|
|
} |
|
|
|
|
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>(); |
|
|
|
|
|
|
|
|
|
double DownhillSolverImpl::minimize(InputOutputArray x){ |
|
|
|
|
dprintf(("hi from minimize\n")); |
|
|
|
|
CV_Assert(_Function.empty()==false); |
|
|
|
|
dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon)); |
|
|
|
|
dprintf(("step\n")); |
|
|
|
|
print_matrix(_step); |
|
|
|
|
for( j = 0; j < ndim; j++ ) |
|
|
|
|
ptry_[j] = coord_sum_[j]*alpha - p_ihi[j]*beta; |
|
|
|
|
|
|
|
|
|
Mat x_mat=x.getMat(); |
|
|
|
|
CV_Assert(MIN(x_mat.rows,x_mat.cols)==1); |
|
|
|
|
CV_Assert(MAX(x_mat.rows,x_mat.cols)==_step.cols); |
|
|
|
|
CV_Assert(x_mat.type()==CV_64FC1); |
|
|
|
|
fcount++; |
|
|
|
|
return calc_f(ptry_); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Mat_<double> proxy_x; |
|
|
|
|
void replacePoint( Mat& p, Mat& coord_sum, Mat& y, int ihi, double alpha_, double ytry ) |
|
|
|
|
{ |
|
|
|
|
int j, ndim = p.cols; |
|
|
|
|
|
|
|
|
|
if(x_mat.rows>1){ |
|
|
|
|
buf_x.create(1,_step.cols); |
|
|
|
|
Mat_<double> proxy(_step.cols,1,buf_x.ptr<double>()); |
|
|
|
|
x_mat.copyTo(proxy); |
|
|
|
|
proxy_x=buf_x; |
|
|
|
|
}else{ |
|
|
|
|
proxy_x=x_mat; |
|
|
|
|
} |
|
|
|
|
double alpha = (1.0 - alpha_)/ndim; |
|
|
|
|
double beta = alpha - alpha_; |
|
|
|
|
double* p_ihi = p.ptr<double>(ihi); |
|
|
|
|
double* coord_sum_ = coord_sum.ptr<double>(); |
|
|
|
|
|
|
|
|
|
int count=0; |
|
|
|
|
int ndim=_step.cols; |
|
|
|
|
Mat_<double> simplex=Mat_<double>(ndim+1,ndim,0.0); |
|
|
|
|
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); |
|
|
|
|
} |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
simplex.row(0).copyTo(proxy_x); |
|
|
|
|
createInitialSimplex(simplex,_step); |
|
|
|
|
double res = innerDownhillSimplex( |
|
|
|
|
simplex,_termcrit.epsilon, _termcrit.epsilon, count,_Function,_termcrit.maxCount); |
|
|
|
|
simplex.row(0).copyTo(proxy_x); |
|
|
|
|
|
|
|
|
|
dprintf(("%d iterations done\n",count)); |
|
|
|
|
// 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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if(x_mat.rows>1){ |
|
|
|
|
Mat(x_mat.rows, 1, CV_64F, proxy_x.ptr<double>()).copyTo(x); |
|
|
|
|
} |
|
|
|
|
return res; |
|
|
|
|
} |
|
|
|
|
DownhillSolverImpl::DownhillSolverImpl(){ |
|
|
|
|
_Function=Ptr<Function>(); |
|
|
|
|
_step=Mat_<double>(); |
|
|
|
|
} |
|
|
|
|
Ptr<MinProblemSolver::Function> DownhillSolverImpl::getFunction()const{ |
|
|
|
|
return _Function; |
|
|
|
|
} |
|
|
|
|
void DownhillSolverImpl::setFunction(const Ptr<Function>& f){ |
|
|
|
|
_Function=f; |
|
|
|
|
} |
|
|
|
|
TermCriteria DownhillSolverImpl::getTermCriteria()const{ |
|
|
|
|
return _termcrit; |
|
|
|
|
} |
|
|
|
|
void DownhillSolverImpl::setTermCriteria(const TermCriteria& termcrit){ |
|
|
|
|
CV_Assert(termcrit.type==(TermCriteria::MAX_ITER+TermCriteria::EPS) && termcrit.epsilon>0 && termcrit.maxCount>0); |
|
|
|
|
_termcrit=termcrit; |
|
|
|
|
} |
|
|
|
|
// 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; |
|
|
|
|
} |
|
|
|
|
void DownhillSolverImpl::getInitStep(OutputArray step)const{ |
|
|
|
|
_step.copyTo(step); |
|
|
|
|
} |
|
|
|
|
void DownhillSolverImpl::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)); |
|
|
|
|
CV_Assert(MIN(m.cols,m.rows)==1 && m.type()==CV_64FC1); |
|
|
|
|
if(m.rows==1){ |
|
|
|
|
m.copyTo(_step); |
|
|
|
|
}else{ |
|
|
|
|
transpose(m,_step); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
Vadim Pisarevsky
10 years ago