Templates -- Meow: meowpp/math/methods.h Source File

@@ -71,7 +71,7 @@ $(document).ready(function(){initNavTree('methods_8h_source.html','');});
  namespace meow {
 
 template<class Data, class WeightingClass>

- inline std::vector<Data> ransac(std::vector<Data> const& data,

+ inline std::vector<Data> ransac(std::vector<Data> const& data,
                                 WeightingClass const&    w,
                                 size_t                   N,
                                 double p0, double P) {

@@ -108,26 +108,154 @@ $(document).ready(function(){initNavTree('methods_8h_source.html','');});
  }
 
- 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());

+ /*

+  * @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;
 }
 
- }

- 

- #endif // math_methods_H__

+ // 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__

+
+
+
+
+
+
+
+
+
+
+
+