diff options
Diffstat (limited to 'meowpp/math/methods.h')
| -rw-r--r-- | meowpp/math/methods.h | 75 |
1 files changed, 62 insertions, 13 deletions
diff --git a/meowpp/math/methods.h b/meowpp/math/methods.h index 4a47d33..5b67594 100644 --- a/meowpp/math/methods.h +++ b/meowpp/math/methods.h @@ -92,7 +92,7 @@ inline std::vector<Data> ransac(std::vector<Data> const& data, } -/*! +/* * @brief Run the \b Levenberg-Marquardt method to solve a non-linear * least squares problem. * @@ -159,23 +159,72 @@ inline std::vector<Data> ransac(std::vector<Data> const& data, * * @author cat_leopard */ -template<class Scalar, class F, class J, class I, class Stop> -inline Vector<Scalar> levenbergMarquardt(F const& func, - J const& jaco, - I const& iden, +template<class Scalar, class Function> +inline Vector<Scalar> levenbergMarquardt(Function const& f, 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()); + 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__ |