#ifndef math_methods_H__
#define math_methods_H__
#include "Matrix.h"
#include "Vector.h"
#include "utility.h"
#include <cstdlib>
#include <vector>
namespace meow {
/*!
* @brief Run the \b RANSAC method to approach the best solution.
*
* \b RANdom \b SAmple \b Consensus is an iterative method to estimate
* parameters of a mathematical model from a set of observed data which
* contains \c outliers. \n
* Each iterator it will choose a subset of elements, the smallest set which can
* form a valid parameters, from the data set. And then calculate how many
* elements in the whole data set is inliers. After iterator much times,
* we just say the best solution is the parameters that has the much
* inliers elements in whole iterators.
*
* Assume:
* - We need at least \f$ N \f$ element to form a valid parameters.
* - The probability of choosing a right element from data set each time is
* \f$ p_0 \f$.
* - We want the probability of our solution actually being the best solution
* be \f$ P \f$.
* - We need to iterator \f$ M \f$ times.
* .
* Then we can estimate the number of iterations \f$ M \f$ :
* \f[
* \begin{aligned}
* & (1 - p_0^N)^M \leq(1 - P) \\
* \Rightarrow & M \log(1 - p_0^N) \leq \log(1 - P) \\
* \Rightarrow & M \geq \frac{\log(1 - p)}{\log(1 - p_0^N)},~~
* \because (1-p_0^N<1 \Rightarrow \log(1-p_0^N)<0)
* \end{aligned}
* \f]
*
* So in this function we choose
* \f$ M = \lceil \frac{\log(1 - P)}{\log(1 - p_0^N)} \rceil \f$
*
* @param [in] data The whole data sett
* @param [in] w Weight function to give a floating number for a given
* parameters which means how best this solution is. Negitave
* number means invalid parameters.
* @param [in] N \f$ N \f$, defined above
* @param [in] p0 \f$ p_0 \f$, defined above
* @param [in] P \f$ P \f$, defined above
* @return solution.
*
* @author cat_leopard
*/
template<class Data, class WeightingClass>
inline std::vector<Data> ransac(std::vector<Data> const& data,
WeightingClass const& w,
size_t N,
double p0, double P) {
if (data.size() < N) {
return std::vector<Data>();
}
double ww = -1.0;
std::vector<Data> ret;
for (double count = ceil(log(1.0 - P) / log(1.0 - pow(p0, N)));
count > 0.0; count -= 1.0) {
std::vector<Data> sample;
std::vector<int> index(N);
for (size_t i = 0; i < N; i++) {
for (bool ok = false; !ok; ) {
index[i] = rand() % data.size();
ok = true;
for (size_t j = 0; ok && j < i; j++)
if (index[i] == index[j])
ok = false;
}
sample.push_back(data[index[i]]);
}
double w_now = w(sample, data);
if (w_now < 0) {
count += 0.5;
continue;
}
if (ww < w_now) {
ret = sample;
ww = w_now;
}
}
return ret;
}
/*
* @brief Run the \b Levenberg-Marquardt method to solve a non-linear
* least squares problem.
*
* Assume:
* - The function we want to optimize is
* \f$ F: \mathbb{R} ^N \mapsto \mathbb{R}^M \f$
* - We want to find the best solution \f$ v \f$ such that
* \f$ F(v)^T F(v) = 0\f$. But there is a gived threshold
* \f$ \epsilon \f$, we can just find a \f$ v \f$ such that
* \f$ F(v)^T F(v) < \epsilon \f$, which is mush easier.
* - User gived a initiial vector \f$ v_0 \f$
* .
* Then we just iteratilly find \f$ v_1, v_2, v_3, v_4... \f$ until a
* vector \f$ v_k \f$ satisified that \f$ F(v_k)^TF(v_k)<\epsilon \f$ .
* And each iterator we have:
* \f[
* v_{i+1} = v_i + (J(v_i)^TJ(v_i)+\lambda I_{N\times N})^{-1} J(v_i)^T F(v_i)
* \f]
* Where \f$ J(v) \f$ is a jacobian matrix defined below:
* \f[
* J(v) = \frac{d}{dv}F(v) =
* \left[ \begin{array}{ccccc}
* \frac{\partial F_1(v)}{\partial v_1} &
* \frac{\partial F_1(v)}{\partial v_2} &
* \frac{\partial F_1(v)}{\partial v_3} &
* ... &
* \frac{\partial F_1(v)}{\partial v_N} \\
* \frac{\partial F_2(v)}{\partial v_1} &
* \frac{\partial F_2(v)}{\partial v_2} &
* \frac{\partial F_2(v)}{\partial v_3} &
* ... &
* \frac{\partial F_2(v)}{\partial v_N} \\
* \frac{\partial F_3(v)}{\partial v_1} &
* \frac{\partial F_3(v)}{\partial v_2} &
* \frac{\partial F_3(v)}{\partial v_3} &
* ... &
* \frac{\partial F_3(v)}{\partial v_N} \\
* . & . & . & & . \\
* . & . & . & & . \\
* . & . & . & & . \\
* \frac{\partial F_M(v)}{\partial v_1} &
* \frac{\partial F_M(v)}{\partial v_2} &
* \frac{\partial F_M(v)}{\partial v_3} &
* ... &
* \frac{\partial F_M(v)}{\partial v_N} \\
* \end{array} \right]
* \f]
* And \f$ \lambda \f$ is a magic number....
* @param [in] func \f$ F \f$, a function(class with \c operator() )
* which input a vector and the output the squares errors.
* @param [in] jaco \f$ J \f$, a function which input a vector
* and then output \b func derivate by the vector
* @param [in] iden \f$ \lambda I_{N \times N} \f$, defined above
* @param [in] init \f$ v_0 \f$Initial vector
* @param [in] stop A function return a boolean which means the error is
* acceptable or not, so \f[
* S_{top}(v) = \begin{cases}
* true & if~F(v)<\epsilon \\
* false & else
* \end{cases}
* \f]
* @param [in] counter To prevent infinit loop.
* @return a vector which means the best solution this function found.
*
* @author cat_leopard
*/
template<class Scalar, class Function>
inline Vector<Scalar> levenbergMarquardt(Function const& f,
Vector<Scalar> const& init,
int counter = -1) {
Vector<Scalar> ans(init), residure_v;
for ( ; counter != 0 && !f.accept(residure_v = f.residure(ans)); --counter) {
Matrix<Scalar> m_j (f.jacobian(ans));
Matrix<Scalar> m_jt(m_j.transpose());
Matrix<Scalar> m(m_j * m_jt), M;
for (int i = 1; M.valid() == false; i++) {
M = (m + f.diagonal(ans, i)).inverse();
}
ans = ans - M * m_jt * residure_v;
}
return ans;
}
// residure
// jacobian
// identity
template<class Scalar, class Function>
inline Vector<Scalar> levenbergMarquardtTraining(Function & f,
Vector<Scalar> const& init,
Scalar const& init_mu,
Scalar const& mu_pow,
Scalar const& er_max,
int retry_number,
int counter) {
if (retry_number == 0) retry_number = 1;
Vector<Scalar> ans_now(init), rv_now(f.residure(ans_now));
Vector<Scalar> ans_nxt , rv_nxt;
Scalar er_now(rv_now.length2());
Scalar er_nxt;
Vector<Scalar> ans_best(ans_now);
Scalar er_best ( er_now);
Matrix<Scalar> m_ja, m_jt, m, iden(f.identity());
Scalar mu(init_mu);
for ( ; counter != 0 && er_now > er_max; --counter) {
m_ja = f.jacobian();
m_jt = m_ja.transpose();
m = m_jt * m_ja;
bool good = false;
for (int i = 0; i != retry_number; ++i, mu = mu * mu_pow) {
ans_nxt = ans_now + (m + iden * mu).inverse() * m_jt * rv_now.matrix();
rv_nxt = f.residure(ans_nxt);
er_nxt = rv_nxt.length2();
if (er_nxt <= er_now) {
good = true;
break;
}
}
if (good) {
mu = mu / mu_pow;
}
mu = inRange(0.0000001, 100.0, mu);
ans_now = ans_nxt;
rv_now = rv_nxt;
er_now = er_nxt;
if (er_now < er_best) {
ans_best = ans_now;
er_best = er_now;
}
}
return ans_best;
}
} // meow
#endif // math_methods_H__
