Joe Verbout
/
main
opencv on mbed
Embed:
(wiki syntax)
Show/hide line numbers
simplex_downhill.h
00001 /*********************************************************************** 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 00005 * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 00006 * 00007 * THE BSD LICENSE 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * 1. Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * 2. Redistributions in binary form must reproduce the above copyright 00016 * notice, this list of conditions and the following disclaimer in the 00017 * documentation and/or other materials provided with the distribution. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 00020 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 00021 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 00022 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 00023 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 00024 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00025 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 00026 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 00028 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00029 *************************************************************************/ 00030 00031 #ifndef OPENCV_FLANN_SIMPLEX_DOWNHILL_H_ 00032 #define OPENCV_FLANN_SIMPLEX_DOWNHILL_H_ 00033 00034 namespace cvflann 00035 { 00036 00037 /** 00038 Adds val to array vals (and point to array points) and keeping the arrays sorted by vals. 00039 */ 00040 template <typename T> 00041 void addValue(int pos, float val, float* vals, T* point, T* points, int n) 00042 { 00043 vals[pos] = val; 00044 for (int i=0; i<n; ++i) { 00045 points[pos*n+i] = point[i]; 00046 } 00047 00048 // bubble down 00049 int j=pos; 00050 while (j>0 && vals[j]<vals[j-1]) { 00051 swap(vals[j],vals[j-1]); 00052 for (int i=0; i<n; ++i) { 00053 swap(points[j*n+i],points[(j-1)*n+i]); 00054 } 00055 --j; 00056 } 00057 } 00058 00059 00060 /** 00061 Simplex downhill optimization function. 00062 Preconditions: points is a 2D mattrix of size (n+1) x n 00063 func is the cost function taking n an array of n params and returning float 00064 vals is the cost function in the n+1 simplex points, if NULL it will be computed 00065 00066 Postcondition: returns optimum value and points[0..n] are the optimum parameters 00067 */ 00068 template <typename T, typename F> 00069 float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL ) 00070 { 00071 const int MAX_ITERATIONS = 10; 00072 00073 assert(n>0); 00074 00075 T* p_o = new T[n]; 00076 T* p_r = new T[n]; 00077 T* p_e = new T[n]; 00078 00079 int alpha = 1; 00080 00081 int iterations = 0; 00082 00083 bool ownVals = false; 00084 if (vals == NULL) { 00085 ownVals = true; 00086 vals = new float[n+1]; 00087 for (int i=0; i<n+1; ++i) { 00088 float val = func(points+i*n); 00089 addValue(i, val, vals, points+i*n, points, n); 00090 } 00091 } 00092 int nn = n*n; 00093 00094 while (true) { 00095 00096 if (iterations++ > MAX_ITERATIONS) break; 00097 00098 // compute average of simplex points (except the highest point) 00099 for (int j=0; j<n; ++j) { 00100 p_o[j] = 0; 00101 for (int i=0; i<n; ++i) { 00102 p_o[i] += points[j*n+i]; 00103 } 00104 } 00105 for (int i=0; i<n; ++i) { 00106 p_o[i] /= n; 00107 } 00108 00109 bool converged = true; 00110 for (int i=0; i<n; ++i) { 00111 if (p_o[i] != points[nn+i]) { 00112 converged = false; 00113 } 00114 } 00115 if (converged) break; 00116 00117 // trying a reflection 00118 for (int i=0; i<n; ++i) { 00119 p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]); 00120 } 00121 float val_r = func(p_r); 00122 00123 if ((val_r>=vals[0])&&(val_r<vals[n])) { 00124 // reflection between second highest and lowest 00125 // add it to the simplex 00126 Logger::info("Choosing reflection\n"); 00127 addValue(n, val_r,vals, p_r, points, n); 00128 continue; 00129 } 00130 00131 if (val_r<vals[0]) { 00132 // value is smaller than smalest in simplex 00133 00134 // expand some more to see if it drops further 00135 for (int i=0; i<n; ++i) { 00136 p_e[i] = 2*p_r[i]-p_o[i]; 00137 } 00138 float val_e = func(p_e); 00139 00140 if (val_e<val_r) { 00141 Logger::info("Choosing reflection and expansion\n"); 00142 addValue(n, val_e,vals,p_e,points,n); 00143 } 00144 else { 00145 Logger::info("Choosing reflection\n"); 00146 addValue(n, val_r,vals,p_r,points,n); 00147 } 00148 continue; 00149 } 00150 if (val_r>=vals[n]) { 00151 for (int i=0; i<n; ++i) { 00152 p_e[i] = (p_o[i]+points[nn+i])/2; 00153 } 00154 float val_e = func(p_e); 00155 00156 if (val_e<vals[n]) { 00157 Logger::info("Choosing contraction\n"); 00158 addValue(n,val_e,vals,p_e,points,n); 00159 continue; 00160 } 00161 } 00162 { 00163 Logger::info("Full contraction\n"); 00164 for (int j=1; j<=n; ++j) { 00165 for (int i=0; i<n; ++i) { 00166 points[j*n+i] = (points[j*n+i]+points[i])/2; 00167 } 00168 float val = func(points+j*n); 00169 addValue(j,val,vals,points+j*n,points,n); 00170 } 00171 } 00172 } 00173 00174 float bestVal = vals[0]; 00175 00176 delete[] p_r; 00177 delete[] p_o; 00178 delete[] p_e; 00179 if (ownVals) delete[] vals; 00180 00181 return bestVal; 00182 } 00183 00184 } 00185 00186 #endif //OPENCV_FLANN_SIMPLEX_DOWNHILL_H_ 00187
Generated on Tue Jul 12 2022 16:42:40 by 1.7.2