点阵完成，加入opencv

3rdpart/OpenCV/include/opencv2/flann/simplex_downhill.h

@@ -0,0 +1,190 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
+ *************************************************************************/
+
+#ifndef OPENCV_FLANN_SIMPLEX_DOWNHILL_H_
+#define OPENCV_FLANN_SIMPLEX_DOWNHILL_H_
+
+//! @cond IGNORED
+
+namespace cvflann
+{
+
+/**
+    Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
+ */
+template <typename T>
+void addValue(int pos, float val, float* vals, T* point, T* points, int n)
+{
+    vals[pos] = val;
+    for (int i=0; i<n; ++i) {
+        points[pos*n+i] = point[i];
+    }
+
+    // bubble down
+    int j=pos;
+    while (j>0 && vals[j]<vals[j-1]) {
+        swap(vals[j],vals[j-1]);
+        for (int i=0; i<n; ++i) {
+            swap(points[j*n+i],points[(j-1)*n+i]);
+        }
+        --j;
+    }
+}
+
+
+/**
+    Simplex downhill optimization function.
+    Preconditions: points is a 2D mattrix of size (n+1) x n
+                    func is the cost function taking n an array of n params and returning float
+                    vals is the cost function in the n+1 simplex points, if NULL it will be computed
+
+    Postcondition: returns optimum value and points[0..n] are the optimum parameters
+ */
+template <typename T, typename F>
+float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
+{
+    const int MAX_ITERATIONS = 10;
+
+    CV_DbgAssert(n>0);
+
+    T* p_o = new T[n];
+    T* p_r = new T[n];
+    T* p_e = new T[n];
+
+    int alpha = 1;
+
+    int iterations = 0;
+
+    bool ownVals = false;
+    if (vals == NULL) {
+        ownVals = true;
+        vals = new float[n+1];
+        for (int i=0; i<n+1; ++i) {
+            float val = func(points+i*n);
+            addValue(i, val, vals, points+i*n, points, n);
+        }
+    }
+    int nn = n*n;
+
+    while (true) {
+
+        if (iterations++ > MAX_ITERATIONS) break;
+
+        // compute average of simplex points (except the highest point)
+        for (int j=0; j<n; ++j) {
+            p_o[j] = 0;
+            for (int i=0; i<n; ++i) {
+                p_o[i] += points[j*n+i];
+            }
+        }
+        for (int i=0; i<n; ++i) {
+            p_o[i] /= n;
+        }
+
+        bool converged = true;
+        for (int i=0; i<n; ++i) {
+            if (p_o[i] != points[nn+i]) {
+                converged = false;
+            }
+        }
+        if (converged) break;
+
+        // trying a reflection
+        for (int i=0; i<n; ++i) {
+            p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
+        }
+        float val_r = func(p_r);
+
+        if ((val_r>=vals[0])&&(val_r<vals[n])) {
+            // reflection between second highest and lowest
+            // add it to the simplex
+            Logger::info("Choosing reflection\n");
+            addValue(n, val_r,vals, p_r, points, n);
+            continue;
+        }
+
+        if (val_r<vals[0]) {
+            // value is smaller than smallest in simplex
+
+            // expand some more to see if it drops further
+            for (int i=0; i<n; ++i) {
+                p_e[i] = 2*p_r[i]-p_o[i];
+            }
+            float val_e = func(p_e);
+
+            if (val_e<val_r) {
+                Logger::info("Choosing reflection and expansion\n");
+                addValue(n, val_e,vals,p_e,points,n);
+            }
+            else {
+                Logger::info("Choosing reflection\n");
+                addValue(n, val_r,vals,p_r,points,n);
+            }
+            continue;
+        }
+        if (val_r>=vals[n]) {
+            for (int i=0; i<n; ++i) {
+                p_e[i] = (p_o[i]+points[nn+i])/2;
+            }
+            float val_e = func(p_e);
+
+            if (val_e<vals[n]) {
+                Logger::info("Choosing contraction\n");
+                addValue(n,val_e,vals,p_e,points,n);
+                continue;
+            }
+        }
+        {
+            Logger::info("Full contraction\n");
+            for (int j=1; j<=n; ++j) {
+                for (int i=0; i<n; ++i) {
+                    points[j*n+i] = (points[j*n+i]+points[i])/2;
+                }
+                float val = func(points+j*n);
+                addValue(j,val,vals,points+j*n,points,n);
+            }
+        }
+    }
+
+    float bestVal = vals[0];
+
+    delete[] p_r;
+    delete[] p_o;
+    delete[] p_e;
+    if (ownVals) delete[] vals;
+
+    return bestVal;
+}
+
+}
+
+//! @endcond
+
+#endif //OPENCV_FLANN_SIMPLEX_DOWNHILL_H_