/***********************************************************************
|
* 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;
|
|
assert(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 smalest 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_
|
