1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 14 // Third party copyrights are property of their respective owners. 15 // 16 // Redistribution and use in source and binary forms, with or without modification, 17 // are permitted provided that the following conditions are met: 18 // 19 // * Redistribution's of source code must retain the above copyright notice, 20 // this list of conditions and the following disclaimer. 21 // 22 // * Redistribution's in binary form must reproduce the above copyright notice, 23 // this list of conditions and the following disclaimer in the documentation 24 // and/or other materials provided with the distribution. 25 // 26 // * The name of the copyright holders may not be used to endorse or promote products 27 // derived from this software without specific prior written permission. 28 // 29 // This software is provided by the copyright holders and contributors "as is" and 30 // any express or implied warranties, including, but not limited to, the implied 31 // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 // In no event shall the OpenCV Foundation or contributors be liable for any direct, 33 // indirect, incidental, special, exemplary, or consequential damages 34 // (including, but not limited to, procurement of substitute goods or services; 35 // loss of use, data, or profits; or business interruption) however caused 36 // and on any theory of liability, whether in contract, strict liability, 37 // or tort (including negligence or otherwise) arising in any way out of 38 // the use of this software, even if advised of the possibility of such damage. 39 // 40 //M*/ 41 #include "precomp.hpp" 42 43 #if 0 44 #define dprintf(x) printf x 45 #define print_matrix(x) print(x) 46 #else 47 #define dprintf(x) 48 #define print_matrix(x) 49 #endif 50 51 /* 52 53 ****Error Message******************************************************************************************************************** 54 55 Downhill Simplex method in OpenCV dev 3.0.0 getting this error: 56 57 OpenCV Error: Assertion failed (dims <= 2 && data && (unsigned)i0 < (unsigned)(s ize.p[0] * size.p[1]) 58 && elemSize() == (((((DataType<_Tp>::type) & ((512 - 1) << 3)) >> 3) + 1) << ((((sizeof(size_t)/4+1)16384|0x3a50) 59 >> ((DataType<_Tp>::typ e) & ((1 << 3) - 1))2) & 3))) in Mat::at, 60 file C:\builds\master_PackSlave-w in32-vc12-shared\opencv\modules\core\include\opencv2/core/mat.inl.hpp, line 893 61 62 ****Problem and Possible Fix********************************************************************************************************* 63 64 DownhillSolverImpl::innerDownhillSimplex something looks broken here: 65 Mat_<double> coord_sum(1,ndim,0.0),buf(1,ndim,0.0),y(1,ndim,0.0); 66 fcount = 0; 67 for(i=0;i<ndim+1;++i) 68 { 69 y(i) = f->calc(p[i]); 70 } 71 72 y has only ndim elements, while the loop goes over ndim+1 73 74 Edited the following for possible fix: 75 76 Replaced y(1,ndim,0.0) ------> y(1,ndim+1,0.0) 77 78 *********************************************************************************************************************************** 79 80 The code below was used in tesing the source code. 81 Created by @SareeAlnaghy 82 83 #include <iostream> 84 #include <cstdlib> 85 #include <cmath> 86 #include <algorithm> 87 #include <opencv2\optim\optim.hpp> 88 89 using namespace std; 90 using namespace cv; 91 92 void test(Ptr<optim::DownhillSolver> MinProblemSolver, Ptr<optim::MinProblemSolver::Function> ptr_F, Mat &P, Mat &step) 93 { 94 try{ 95 96 MinProblemSolver->setFunction(ptr_F); 97 MinProblemSolver->setInitStep(step); 98 double res = MinProblemSolver->minimize(P); 99 100 cout << "res " << res << endl; 101 } 102 catch (exception e) 103 { 104 cerr << "Error:: " << e.what() << endl; 105 } 106 } 107 108 int main() 109 { 110 111 class DistanceToLines :public optim::MinProblemSolver::Function { 112 public: 113 double calc(const double* x)const{ 114 115 return x[0] * x[0] + x[1] * x[1]; 116 117 } 118 }; 119 120 Mat P = (Mat_<double>(1, 2) << 1.0, 1.0); 121 Mat step = (Mat_<double>(2, 1) << -0.5, 0.5); 122 123 Ptr<optim::MinProblemSolver::Function> ptr_F(new DistanceToLines()); 124 Ptr<optim::DownhillSolver> MinProblemSolver = optim::createDownhillSolver(); 125 126 test(MinProblemSolver, ptr_F, P, step); 127 128 system("pause"); 129 return 0; 130 } 131 132 ****Suggesttion for imporving Simplex implentation*************************************************************************************** 133 134 Currently the downhilll simplex method outputs the function value that is minimized. It should also return the coordinate points where the 135 function is minimized. This is very useful in many applications such as using back projection methods to find a point of intersection of 136 multiple lines in three dimensions as not all lines intersect in three dimensions. 137 138 */ 139 140 namespace cv 141 { 142 143 class DownhillSolverImpl : public DownhillSolver 144 { 145 public: 146 DownhillSolverImpl() 147 { 148 _Function=Ptr<Function>(); 149 _step=Mat_<double>(); 150 } 151 152 void getInitStep(OutputArray step) const { _step.copyTo(step); } 153 void setInitStep(InputArray step) 154 { 155 // set dimensionality and make a deep copy of step 156 Mat m = step.getMat(); 157 dprintf(("m.cols=%d\nm.rows=%d\n", m.cols, m.rows)); 158 if( m.rows == 1 ) 159 m.copyTo(_step); 160 else 161 transpose(m, _step); 162 } 163 164 Ptr<MinProblemSolver::Function> getFunction() const { return _Function; } 165 166 void setFunction(const Ptr<Function>& f) { _Function=f; } 167 168 TermCriteria getTermCriteria() const { return _termcrit; } 169 170 void setTermCriteria( const TermCriteria& termcrit ) 171 { 172 CV_Assert( termcrit.type == (TermCriteria::MAX_ITER + TermCriteria::EPS) && 173 termcrit.epsilon > 0 && 174 termcrit.maxCount > 0 ); 175 _termcrit=termcrit; 176 } 177 178 double minimize( InputOutputArray x_ ) 179 { 180 dprintf(("hi from minimize\n")); 181 CV_Assert( !_Function.empty() ); 182 CV_Assert( std::min(_step.cols, _step.rows) == 1 && 183 std::max(_step.cols, _step.rows) >= 2 && 184 _step.type() == CV_64FC1 ); 185 dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon)); 186 dprintf(("step\n")); 187 print_matrix(_step); 188 189 Mat x = x_.getMat(), simplex; 190 191 createInitialSimplex(x, simplex, _step); 192 int count = 0; 193 double res = innerDownhillSimplex(simplex,_termcrit.epsilon, _termcrit.epsilon, 194 count, _termcrit.maxCount); 195 dprintf(("%d iterations done\n",count)); 196 197 if( !x.empty() ) 198 { 199 Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>()); 200 simplex_0m.convertTo(x, x.type()); 201 } 202 else 203 { 204 int x_type = x_.fixedType() ? x_.type() : CV_64F; 205 simplex.row(0).convertTo(x_, x_type); 206 } 207 return res; 208 } 209 protected: 210 Ptr<MinProblemSolver::Function> _Function; 211 TermCriteria _termcrit; 212 Mat _step; 213 214 inline void updateCoordSum(const Mat& p, Mat& coord_sum) 215 { 216 int i, j, m = p.rows, n = p.cols; 217 double* coord_sum_ = coord_sum.ptr<double>(); 218 CV_Assert( coord_sum.cols == n && coord_sum.rows == 1 ); 219 220 for( j = 0; j < n; j++ ) 221 coord_sum_[j] = 0.; 222 223 for( i = 0; i < m; i++ ) 224 { 225 const double* p_i = p.ptr<double>(i); 226 for( j = 0; j < n; j++ ) 227 coord_sum_[j] += p_i[j]; 228 } 229 230 dprintf(("\nupdated coord sum:\n")); 231 print_matrix(coord_sum); 232 233 } 234 235 inline void createInitialSimplex( const Mat& x0, Mat& simplex, Mat& step ) 236 { 237 int i, j, ndim = step.cols; 238 CV_Assert( _Function->getDims() == ndim ); 239 Mat x = x0; 240 if( x0.empty() ) 241 x = Mat::zeros(1, ndim, CV_64F); 242 CV_Assert( (x.cols == 1 && x.rows == ndim) || (x.cols == ndim && x.rows == 1) ); 243 CV_Assert( x.type() == CV_32F || x.type() == CV_64F ); 244 245 simplex.create(ndim + 1, ndim, CV_64F); 246 Mat simplex_0m(x.rows, x.cols, CV_64F, simplex.ptr<double>()); 247 248 x.convertTo(simplex_0m, CV_64F); 249 double* simplex_0 = simplex.ptr<double>(); 250 const double* step_ = step.ptr<double>(); 251 for( i = 1; i <= ndim; i++ ) 252 { 253 double* simplex_i = simplex.ptr<double>(i); 254 for( j = 0; j < ndim; j++ ) 255 simplex_i[j] = simplex_0[j]; 256 simplex_i[i-1] += 0.5*step_[i-1]; 257 } 258 for( j = 0; j < ndim; j++ ) 259 simplex_0[j] -= 0.5*step_[j]; 260 261 dprintf(("\nthis is simplex\n")); 262 print_matrix(simplex); 263 } 264 265 /* 266 Performs the actual minimization of MinProblemSolver::Function f (after the initialization was done) 267 268 The matrix p[ndim+1][1..ndim] represents ndim+1 vertices that 269 form a simplex - each row is an ndim vector. 270 On output, fcount gives the number of function evaluations taken. 271 */ 272 double innerDownhillSimplex( Mat& p, double MinRange, double MinError, int& fcount, int nmax ) 273 { 274 int i, j, ndim = p.cols; 275 Mat coord_sum(1, ndim, CV_64F), buf(1, ndim, CV_64F), y(1, ndim+1, CV_64F); 276 double* y_ = y.ptr<double>(); 277 278 fcount = ndim+1; 279 for( i = 0; i <= ndim; i++ ) 280 y_[i] = calc_f(p.ptr<double>(i)); 281 282 updateCoordSum(p, coord_sum); 283 284 for (;;) 285 { 286 // find highest (worst), next-to-worst, and lowest 287 // (best) points by going through all of them. 288 int ilo = 0, ihi, inhi; 289 if( y_[0] > y_[1] ) 290 { 291 ihi = 0; inhi = 1; 292 } 293 else 294 { 295 ihi = 1; inhi = 0; 296 } 297 for( i = 0; i <= ndim; i++ ) 298 { 299 double yval = y_[i]; 300 if (yval <= y_[ilo]) 301 ilo = i; 302 if (yval > y_[ihi]) 303 { 304 inhi = ihi; 305 ihi = i; 306 } 307 else if (yval > y_[inhi] && i != ihi) 308 inhi = i; 309 } 310 CV_Assert( ihi != inhi ); 311 if( ilo == inhi || ilo == ihi ) 312 { 313 for( i = 0; i <= ndim; i++ ) 314 { 315 double yval = y_[i]; 316 if( yval == y_[ilo] && i != ihi && i != inhi ) 317 { 318 ilo = i; 319 break; 320 } 321 } 322 } 323 dprintf(("\nthis is y on iteration %d:\n",fcount)); 324 print_matrix(y); 325 326 // check stop criterion 327 double error = fabs(y_[ihi] - y_[ilo]); 328 double range = 0; 329 for( j = 0; j < ndim; j++ ) 330 { 331 double minval, maxval; 332 minval = maxval = p.at<double>(0, j); 333 for( i = 1; i <= ndim; i++ ) 334 { 335 double pval = p.at<double>(i, j); 336 minval = std::min(minval, pval); 337 maxval = std::max(maxval, pval); 338 } 339 range = std::max(range, fabs(maxval - minval)); 340 } 341 342 if( range <= MinRange || error <= MinError || fcount >= nmax ) 343 { 344 // Put best point and value in first slot. 345 std::swap(y_[0], y_[ilo]); 346 for( j = 0; j < ndim; j++ ) 347 { 348 std::swap(p.at<double>(0, j), p.at<double>(ilo, j)); 349 } 350 break; 351 } 352 353 double y_lo = y_[ilo], y_nhi = y_[inhi], y_hi = y_[ihi]; 354 // Begin a new iteration. First, reflect the worst point about the centroid of others 355 double alpha = -1.0; 356 double y_alpha = tryNewPoint(p, coord_sum, ihi, alpha, buf, fcount); 357 358 dprintf(("\ny_lo=%g, y_nhi=%g, y_hi=%g, y_alpha=%g, p_alpha:\n", y_lo, y_nhi, y_hi, y_alpha)); 359 print_matrix(buf); 360 361 if( y_alpha < y_nhi ) 362 { 363 if( y_alpha < y_lo ) 364 { 365 // If that's better than the best point, go twice as far in that direction 366 double beta = -2.0; 367 double y_beta = tryNewPoint(p, coord_sum, ihi, beta, buf, fcount); 368 dprintf(("\ny_beta=%g, p_beta:\n", y_beta)); 369 print_matrix(buf); 370 if( y_beta < y_alpha ) 371 { 372 alpha = beta; 373 y_alpha = y_beta; 374 } 375 } 376 replacePoint(p, coord_sum, y, ihi, alpha, y_alpha); 377 } 378 else 379 { 380 // The new point is worse than the second-highest, 381 // do not go so far in that direction 382 double gamma = 0.5; 383 double y_gamma = tryNewPoint(p, coord_sum, ihi, gamma, buf, fcount); 384 dprintf(("\ny_gamma=%g, p_gamma:\n", y_gamma)); 385 print_matrix(buf); 386 if( y_gamma < y_hi ) 387 replacePoint(p, coord_sum, y, ihi, gamma, y_gamma); 388 else 389 { 390 // Can't seem to improve things. 391 // Contract the simplex to good point 392 // in hope to find a simplex landscape. 393 for( i = 0; i <= ndim; i++ ) 394 { 395 if (i != ilo) 396 { 397 for( j = 0; j < ndim; j++ ) 398 p.at<double>(i, j) = 0.5*(p.at<double>(i, j) + p.at<double>(ilo, j)); 399 y_[i] = calc_f(p.ptr<double>(i)); 400 } 401 } 402 fcount += ndim; 403 updateCoordSum(p, coord_sum); 404 } 405 } 406 dprintf(("\nthis is simplex on iteration %d\n",fcount)); 407 print_matrix(p); 408 } 409 return y_[0]; 410 } 411 412 inline double calc_f(const double* ptr) 413 { 414 double res = _Function->calc(ptr); 415 CV_Assert( !cvIsNaN(res) && !cvIsInf(res) ); 416 return res; 417 } 418 419 double tryNewPoint( Mat& p, Mat& coord_sum, int ihi, double alpha_, Mat& ptry, int& fcount ) 420 { 421 int j, ndim = p.cols; 422 423 double alpha = (1.0 - alpha_)/ndim; 424 double beta = alpha - alpha_; 425 double* p_ihi = p.ptr<double>(ihi); 426 double* ptry_ = ptry.ptr<double>(); 427 double* coord_sum_ = coord_sum.ptr<double>(); 428 429 for( j = 0; j < ndim; j++ ) 430 ptry_[j] = coord_sum_[j]*alpha - p_ihi[j]*beta; 431 432 fcount++; 433 return calc_f(ptry_); 434 } 435 436 void replacePoint( Mat& p, Mat& coord_sum, Mat& y, int ihi, double alpha_, double ytry ) 437 { 438 int j, ndim = p.cols; 439 440 double alpha = (1.0 - alpha_)/ndim; 441 double beta = alpha - alpha_; 442 double* p_ihi = p.ptr<double>(ihi); 443 double* coord_sum_ = coord_sum.ptr<double>(); 444 445 for( j = 0; j < ndim; j++ ) 446 p_ihi[j] = coord_sum_[j]*alpha - p_ihi[j]*beta; 447 y.at<double>(ihi) = ytry; 448 updateCoordSum(p, coord_sum); 449 } 450 }; 451 452 453 // both minRange & minError are specified by termcrit.epsilon; 454 // In addition, user may specify the number of iterations that the algorithm does. 455 Ptr<DownhillSolver> DownhillSolver::create( const Ptr<MinProblemSolver::Function>& f, 456 InputArray initStep, TermCriteria termcrit ) 457 { 458 Ptr<DownhillSolver> DS = makePtr<DownhillSolverImpl>(); 459 DS->setFunction(f); 460 DS->setInitStep(initStep); 461 DS->setTermCriteria(termcrit); 462 return DS; 463 } 464 465 } 466
