methods.h

Go to the documentation of this file.

#ifndef   math_methods_H__
#define   math_methods_H__

#include "Matrix.h"
#include "Vector.h"
#include "utility.h"

#include <cstdlib>
#include <vector>

namespace meow {

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;
}


template<class Scalar, class F, class J, class I, class Stop>
inline Vector<Scalar> levenbergMarquardt(F const& func,
                                         J const& jaco,
                                         I const& iden,
                                         Vector<Scalar> const& init,
                                         Stop           const& stop,
                                         int counter = -1) {
  Vector<Scalar> ans(init);
  for(Vector<Scalar> rv;
      !stop((rv = func(ans)).length2()) && counter != 0; counter--) {
    Matrix<Scalar> j(jaco(ans)), jt(j.transpose());
    Matrix<Scalar> i(iden(ans));
    ans = ans - Vector<Scalar>((jt * j + i).inverse() * jt * rv.matrix());
  }
  return ans;
}

}

#endif // math_methods_H__