Renesas / opencv-lib

openCV library for Renesas RZ/A

Dependents:   RZ_A2M_Mbed_samples

include/opencv2/flann/simplex_downhill.h@0:0e0631af0305, 2021-01-29 (annotated)

Committer:
RyoheiHagimoto
Date:
Fri Jan 29 04:53:38 2021 +0000
Revision:
0:0e0631af0305
copied from https://github.com/d-kato/opencv-lib.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RyoheiHagimoto 0:0e0631af0305 1 /***********************************************************************
RyoheiHagimoto 0:0e0631af0305 2 * Software License Agreement (BSD License)
RyoheiHagimoto 0:0e0631af0305 3 *
RyoheiHagimoto 0:0e0631af0305 4 * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
RyoheiHagimoto 0:0e0631af0305 5 * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
RyoheiHagimoto 0:0e0631af0305 6 *
RyoheiHagimoto 0:0e0631af0305 7 * THE BSD LICENSE
RyoheiHagimoto 0:0e0631af0305 8 *
RyoheiHagimoto 0:0e0631af0305 9 * Redistribution and use in source and binary forms, with or without
RyoheiHagimoto 0:0e0631af0305 10 * modification, are permitted provided that the following conditions
RyoheiHagimoto 0:0e0631af0305 11 * are met:
RyoheiHagimoto 0:0e0631af0305 12 *
RyoheiHagimoto 0:0e0631af0305 13 * 1. Redistributions of source code must retain the above copyright
RyoheiHagimoto 0:0e0631af0305 14 * notice, this list of conditions and the following disclaimer.
RyoheiHagimoto 0:0e0631af0305 15 * 2. Redistributions in binary form must reproduce the above copyright
RyoheiHagimoto 0:0e0631af0305 16 * notice, this list of conditions and the following disclaimer in the
RyoheiHagimoto 0:0e0631af0305 17 * documentation and/or other materials provided with the distribution.
RyoheiHagimoto 0:0e0631af0305 18 *
RyoheiHagimoto 0:0e0631af0305 19 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
RyoheiHagimoto 0:0e0631af0305 20 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
RyoheiHagimoto 0:0e0631af0305 21 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
RyoheiHagimoto 0:0e0631af0305 22 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
RyoheiHagimoto 0:0e0631af0305 23 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
RyoheiHagimoto 0:0e0631af0305 24 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
RyoheiHagimoto 0:0e0631af0305 25 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
RyoheiHagimoto 0:0e0631af0305 26 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
RyoheiHagimoto 0:0e0631af0305 27 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
RyoheiHagimoto 0:0e0631af0305 28 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RyoheiHagimoto 0:0e0631af0305 29 *************************************************************************/
RyoheiHagimoto 0:0e0631af0305 30
RyoheiHagimoto 0:0e0631af0305 31 #ifndef OPENCV_FLANN_SIMPLEX_DOWNHILL_H_
RyoheiHagimoto 0:0e0631af0305 32 #define OPENCV_FLANN_SIMPLEX_DOWNHILL_H_
RyoheiHagimoto 0:0e0631af0305 33
RyoheiHagimoto 0:0e0631af0305 34 namespace cvflann
RyoheiHagimoto 0:0e0631af0305 35 {
RyoheiHagimoto 0:0e0631af0305 36
RyoheiHagimoto 0:0e0631af0305 37 /**
RyoheiHagimoto 0:0e0631af0305 38 Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
RyoheiHagimoto 0:0e0631af0305 39 */
RyoheiHagimoto 0:0e0631af0305 40 template <typename T>
RyoheiHagimoto 0:0e0631af0305 41 void addValue(int pos, float val, float* vals, T* point, T* points, int n)
RyoheiHagimoto 0:0e0631af0305 42 {
RyoheiHagimoto 0:0e0631af0305 43 vals[pos] = val;
RyoheiHagimoto 0:0e0631af0305 44 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 45 points[pos*n+i] = point[i];
RyoheiHagimoto 0:0e0631af0305 46 }
RyoheiHagimoto 0:0e0631af0305 47
RyoheiHagimoto 0:0e0631af0305 48 // bubble down
RyoheiHagimoto 0:0e0631af0305 49 int j=pos;
RyoheiHagimoto 0:0e0631af0305 50 while (j>0 && vals[j]<vals[j-1]) {
RyoheiHagimoto 0:0e0631af0305 51 swap(vals[j],vals[j-1]);
RyoheiHagimoto 0:0e0631af0305 52 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 53 swap(points[j*n+i],points[(j-1)*n+i]);
RyoheiHagimoto 0:0e0631af0305 54 }
RyoheiHagimoto 0:0e0631af0305 55 --j;
RyoheiHagimoto 0:0e0631af0305 56 }
RyoheiHagimoto 0:0e0631af0305 57 }
RyoheiHagimoto 0:0e0631af0305 58
RyoheiHagimoto 0:0e0631af0305 59
RyoheiHagimoto 0:0e0631af0305 60 /**
RyoheiHagimoto 0:0e0631af0305 61 Simplex downhill optimization function.
RyoheiHagimoto 0:0e0631af0305 62 Preconditions: points is a 2D mattrix of size (n+1) x n
RyoheiHagimoto 0:0e0631af0305 63 func is the cost function taking n an array of n params and returning float
RyoheiHagimoto 0:0e0631af0305 64 vals is the cost function in the n+1 simplex points, if NULL it will be computed
RyoheiHagimoto 0:0e0631af0305 65
RyoheiHagimoto 0:0e0631af0305 66 Postcondition: returns optimum value and points[0..n] are the optimum parameters
RyoheiHagimoto 0:0e0631af0305 67 */
RyoheiHagimoto 0:0e0631af0305 68 template <typename T, typename F>
RyoheiHagimoto 0:0e0631af0305 69 float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
RyoheiHagimoto 0:0e0631af0305 70 {
RyoheiHagimoto 0:0e0631af0305 71 const int MAX_ITERATIONS = 10;
RyoheiHagimoto 0:0e0631af0305 72
RyoheiHagimoto 0:0e0631af0305 73 assert(n>0);
RyoheiHagimoto 0:0e0631af0305 74
RyoheiHagimoto 0:0e0631af0305 75 T* p_o = new T[n];
RyoheiHagimoto 0:0e0631af0305 76 T* p_r = new T[n];
RyoheiHagimoto 0:0e0631af0305 77 T* p_e = new T[n];
RyoheiHagimoto 0:0e0631af0305 78
RyoheiHagimoto 0:0e0631af0305 79 int alpha = 1;
RyoheiHagimoto 0:0e0631af0305 80
RyoheiHagimoto 0:0e0631af0305 81 int iterations = 0;
RyoheiHagimoto 0:0e0631af0305 82
RyoheiHagimoto 0:0e0631af0305 83 bool ownVals = false;
RyoheiHagimoto 0:0e0631af0305 84 if (vals == NULL) {
RyoheiHagimoto 0:0e0631af0305 85 ownVals = true;
RyoheiHagimoto 0:0e0631af0305 86 vals = new float[n+1];
RyoheiHagimoto 0:0e0631af0305 87 for (int i=0; i<n+1; ++i) {
RyoheiHagimoto 0:0e0631af0305 88 float val = func(points+i*n);
RyoheiHagimoto 0:0e0631af0305 89 addValue(i, val, vals, points+i*n, points, n);
RyoheiHagimoto 0:0e0631af0305 90 }
RyoheiHagimoto 0:0e0631af0305 91 }
RyoheiHagimoto 0:0e0631af0305 92 int nn = n*n;
RyoheiHagimoto 0:0e0631af0305 93
RyoheiHagimoto 0:0e0631af0305 94 while (true) {
RyoheiHagimoto 0:0e0631af0305 95
RyoheiHagimoto 0:0e0631af0305 96 if (iterations++ > MAX_ITERATIONS) break;
RyoheiHagimoto 0:0e0631af0305 97
RyoheiHagimoto 0:0e0631af0305 98 // compute average of simplex points (except the highest point)
RyoheiHagimoto 0:0e0631af0305 99 for (int j=0; j<n; ++j) {
RyoheiHagimoto 0:0e0631af0305 100 p_o[j] = 0;
RyoheiHagimoto 0:0e0631af0305 101 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 102 p_o[i] += points[j*n+i];
RyoheiHagimoto 0:0e0631af0305 103 }
RyoheiHagimoto 0:0e0631af0305 104 }
RyoheiHagimoto 0:0e0631af0305 105 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 106 p_o[i] /= n;
RyoheiHagimoto 0:0e0631af0305 107 }
RyoheiHagimoto 0:0e0631af0305 108
RyoheiHagimoto 0:0e0631af0305 109 bool converged = true;
RyoheiHagimoto 0:0e0631af0305 110 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 111 if (p_o[i] != points[nn+i]) {
RyoheiHagimoto 0:0e0631af0305 112 converged = false;
RyoheiHagimoto 0:0e0631af0305 113 }
RyoheiHagimoto 0:0e0631af0305 114 }
RyoheiHagimoto 0:0e0631af0305 115 if (converged) break;
RyoheiHagimoto 0:0e0631af0305 116
RyoheiHagimoto 0:0e0631af0305 117 // trying a reflection
RyoheiHagimoto 0:0e0631af0305 118 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 119 p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
RyoheiHagimoto 0:0e0631af0305 120 }
RyoheiHagimoto 0:0e0631af0305 121 float val_r = func(p_r);
RyoheiHagimoto 0:0e0631af0305 122
RyoheiHagimoto 0:0e0631af0305 123 if ((val_r>=vals[0])&&(val_r<vals[n])) {
RyoheiHagimoto 0:0e0631af0305 124 // reflection between second highest and lowest
RyoheiHagimoto 0:0e0631af0305 125 // add it to the simplex
RyoheiHagimoto 0:0e0631af0305 126 Logger::info("Choosing reflection\n");
RyoheiHagimoto 0:0e0631af0305 127 addValue(n, val_r,vals, p_r, points, n);
RyoheiHagimoto 0:0e0631af0305 128 continue;
RyoheiHagimoto 0:0e0631af0305 129 }
RyoheiHagimoto 0:0e0631af0305 130
RyoheiHagimoto 0:0e0631af0305 131 if (val_r<vals[0]) {
RyoheiHagimoto 0:0e0631af0305 132 // value is smaller than smalest in simplex
RyoheiHagimoto 0:0e0631af0305 133
RyoheiHagimoto 0:0e0631af0305 134 // expand some more to see if it drops further
RyoheiHagimoto 0:0e0631af0305 135 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 136 p_e[i] = 2*p_r[i]-p_o[i];
RyoheiHagimoto 0:0e0631af0305 137 }
RyoheiHagimoto 0:0e0631af0305 138 float val_e = func(p_e);
RyoheiHagimoto 0:0e0631af0305 139
RyoheiHagimoto 0:0e0631af0305 140 if (val_e<val_r) {
RyoheiHagimoto 0:0e0631af0305 141 Logger::info("Choosing reflection and expansion\n");
RyoheiHagimoto 0:0e0631af0305 142 addValue(n, val_e,vals,p_e,points,n);
RyoheiHagimoto 0:0e0631af0305 143 }
RyoheiHagimoto 0:0e0631af0305 144 else {
RyoheiHagimoto 0:0e0631af0305 145 Logger::info("Choosing reflection\n");
RyoheiHagimoto 0:0e0631af0305 146 addValue(n, val_r,vals,p_r,points,n);
RyoheiHagimoto 0:0e0631af0305 147 }
RyoheiHagimoto 0:0e0631af0305 148 continue;
RyoheiHagimoto 0:0e0631af0305 149 }
RyoheiHagimoto 0:0e0631af0305 150 if (val_r>=vals[n]) {
RyoheiHagimoto 0:0e0631af0305 151 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 152 p_e[i] = (p_o[i]+points[nn+i])/2;
RyoheiHagimoto 0:0e0631af0305 153 }
RyoheiHagimoto 0:0e0631af0305 154 float val_e = func(p_e);
RyoheiHagimoto 0:0e0631af0305 155
RyoheiHagimoto 0:0e0631af0305 156 if (val_e<vals[n]) {
RyoheiHagimoto 0:0e0631af0305 157 Logger::info("Choosing contraction\n");
RyoheiHagimoto 0:0e0631af0305 158 addValue(n,val_e,vals,p_e,points,n);
RyoheiHagimoto 0:0e0631af0305 159 continue;
RyoheiHagimoto 0:0e0631af0305 160 }
RyoheiHagimoto 0:0e0631af0305 161 }
RyoheiHagimoto 0:0e0631af0305 162 {
RyoheiHagimoto 0:0e0631af0305 163 Logger::info("Full contraction\n");
RyoheiHagimoto 0:0e0631af0305 164 for (int j=1; j<=n; ++j) {
RyoheiHagimoto 0:0e0631af0305 165 for (int i=0; i<n; ++i) {
RyoheiHagimoto 0:0e0631af0305 166 points[j*n+i] = (points[j*n+i]+points[i])/2;
RyoheiHagimoto 0:0e0631af0305 167 }
RyoheiHagimoto 0:0e0631af0305 168 float val = func(points+j*n);
RyoheiHagimoto 0:0e0631af0305 169 addValue(j,val,vals,points+j*n,points,n);
RyoheiHagimoto 0:0e0631af0305 170 }
RyoheiHagimoto 0:0e0631af0305 171 }
RyoheiHagimoto 0:0e0631af0305 172 }
RyoheiHagimoto 0:0e0631af0305 173
RyoheiHagimoto 0:0e0631af0305 174 float bestVal = vals[0];
RyoheiHagimoto 0:0e0631af0305 175
RyoheiHagimoto 0:0e0631af0305 176 delete[] p_r;
RyoheiHagimoto 0:0e0631af0305 177 delete[] p_o;
RyoheiHagimoto 0:0e0631af0305 178 delete[] p_e;
RyoheiHagimoto 0:0e0631af0305 179 if (ownVals) delete[] vals;
RyoheiHagimoto 0:0e0631af0305 180
RyoheiHagimoto 0:0e0631af0305 181 return bestVal;
RyoheiHagimoto 0:0e0631af0305 182 }
RyoheiHagimoto 0:0e0631af0305 183
RyoheiHagimoto 0:0e0631af0305 184 }
RyoheiHagimoto 0:0e0631af0305 185
RyoheiHagimoto 0:0e0631af0305 186 #endif //OPENCV_FLANN_SIMPLEX_DOWNHILL_H_