<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
<title>CLevenbergMarquardt.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div>
<div align="right">
<a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a>
</div>
<!-- Generated by Doxygen 1.7.5 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="namespaces.html"><span>Namespaces</span></a></li>
<li><a href="annotated.html"><span>Classes</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<div class="left">
<form id="FSearchBox" action="search.php" method="get">
<img id="MSearchSelect" src="search/mag.png" alt=""/>
<input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"/>
</form>
</div><div class="right"></div>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<div class="header">
<div class="headertitle">
<div class="title">CLevenbergMarquardt.h</div> </div>
</div>
<div class="contents">
<a href="_c_levenberg_marquardt_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef CLevenbergMarquardt_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CLevenbergMarquardt_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="_c_debug_output_capable_8h.html">mrpt/utils/CDebugOutputCapable.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_matrix_d_8h.html">mrpt/math/CMatrixD.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="base_2include_2mrpt_2math_2utils_8h.html">mrpt/math/utils.h</a>></span>
<a name="l00034"></a>00034
<a name="l00035"></a>00035 <span class="comment">/*---------------------------------------------------------------</span>
<a name="l00036"></a>00036 <span class="comment"> Class</span>
<a name="l00037"></a>00037 <span class="comment"> ---------------------------------------------------------------*/</span>
<a name="l00038"></a>00038 <span class="keyword">namespace </span>mrpt
<a name="l00039"></a>00039 {
<a name="l00040"></a>00040 <span class="keyword">namespace </span>math
<a name="l00041"></a>00041 {<span class="comment"></span>
<a name="l00042"></a>00042 <span class="comment"> /** An implementation of the Levenberg-Marquardt algorithm for least-square minimization.</span>
<a name="l00043"></a>00043 <span class="comment"> *</span>
<a name="l00044"></a>00044 <span class="comment"> * Refer to this <a href="http://www.mrpt.org/Levenberg%E2%80%93Marquardt_algorithm" >page</a> for more details on the algorithm and its usage.</span>
<a name="l00045"></a>00045 <span class="comment"> *</span>
<a name="l00046"></a>00046 <span class="comment"> * \tparam NUMTYPE The numeric type for all the operations (float, double, or long double)</span>
<a name="l00047"></a>00047 <span class="comment"> * \tparam USERPARAM The type of the "y" input to the user supplied evaluation functor. Default type is a vector of NUMTYPE.</span>
<a name="l00048"></a>00048 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l00049"></a>00049 <span class="comment"> */</span>
<a name="l00050"></a>00050 <span class="keyword">template</span> <<span class="keyword">typename</span> VECTORTYPE = mrpt::vector_<span class="keywordtype">double</span>, <span class="keyword">class</span> USERPARAM = VECTORTYPE >
<a name="l00051"></a>00051 <span class="keyword">class </span>CLevenbergMarquardtTempl : <span class="keyword">public</span> mrpt::utils::CDebugOutputCapable
<a name="l00052"></a>00052 {
<a name="l00053"></a>00053 <span class="keyword">public</span>:
<a name="l00054"></a><a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">00054</a> <span class="keyword">typedef</span> <span class="keyword">typename</span> VECTORTYPE<a class="code" href="eigen__plugins_8h.html#afd07186978da46f9908364e389f8a403" title="Type of the elements.">::value_type</a> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a>;
<a name="l00055"></a>00055
<a name="l00056"></a>00056 <span class="comment"></span>
<a name="l00057"></a>00057 <span class="comment"> /** The type of the function passed to execute. The user must supply a function which evaluates the error of a given point in the solution space.</span>
<a name="l00058"></a>00058 <span class="comment"> * \param x The state point under examination.</span>
<a name="l00059"></a>00059 <span class="comment"> * \param y The same object passed to "execute" as the parameter "userParam".</span>
<a name="l00060"></a>00060 <span class="comment"> * \param out The vector of (non-squared) errors, of the average square root error, for the given "x". The functor code must set the size of this vector.</span>
<a name="l00061"></a>00061 <span class="comment"> */</span>
<a name="l00062"></a><a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#af1417a100aac50f016fcbd86455172fd">00062</a> <span class="keyword">typedef</span> void (*<a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#af1417a100aac50f016fcbd86455172fd" title="The type of the function passed to execute.">TFunctorEval</a>)(
<a name="l00063"></a>00063 <span class="keyword">const</span> VECTORTYPE &x,
<a name="l00064"></a>00064 <span class="keyword">const</span> USERPARAM &<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>,
<a name="l00065"></a>00065 VECTORTYPE &out);
<a name="l00066"></a>00066 <span class="comment"></span>
<a name="l00067"></a>00067 <span class="comment"> /** The type of an optional functor passed to \a execute to replace the Euclidean addition "x_new = x_old + x_incr" by any other operation.</span>
<a name="l00068"></a>00068 <span class="comment"> */</span>
<a name="l00069"></a><a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#ac116d455d54d9d260f78be3264c1cf0e">00069</a> <span class="keyword">typedef</span> void (*<a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#ac116d455d54d9d260f78be3264c1cf0e" title="The type of an optional functor passed to execute to replace the Euclidean addition "x_new = x_old + ...">TFunctorIncrement</a>)(
<a name="l00070"></a>00070 VECTORTYPE &x_new,
<a name="l00071"></a>00071 <span class="keyword">const</span> VECTORTYPE &x_old,
<a name="l00072"></a>00072 <span class="keyword">const</span> VECTORTYPE &x_incr,
<a name="l00073"></a>00073 <span class="keyword">const</span> USERPARAM &user_param);
<a name="l00074"></a>00074
<a name="l00075"></a>00075 <span class="keyword">struct </span><a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html">TResultInfo</a>
<a name="l00076"></a>00076 {
<a name="l00077"></a><a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#accb179389ffd5bde573e798e73b9b9bf">00077</a> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> <a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#accb179389ffd5bde573e798e73b9b9bf">final_sqr_err</a>;
<a name="l00078"></a><a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2e93e60501a6978c38b2204593b3cc66">00078</a> <span class="keywordtype">size_t</span> <a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2e93e60501a6978c38b2204593b3cc66">iterations_executed</a>;
<a name="l00079"></a><a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a8721a85def6d8560eb69789066d70743">00079</a> VECTORTYPE <a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a8721a85def6d8560eb69789066d70743" title="The last error vector returned by the user-provided functor.">last_err_vector</a>; <span class="comment">//!< The last error vector returned by the user-provided functor.</span>
<a name="l00080"></a><a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa">00080</a> <span class="comment"></span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<NUMTYPE></a> <a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>; <span class="comment">//!< Each row is the optimized value at each iteration.</span>
<a name="l00081"></a>00081 <span class="comment"></span><span class="comment"></span>
<a name="l00082"></a>00082 <span class="comment"> /** This matrix can be used to obtain an estimate of the optimal parameters covariance matrix:</span>
<a name="l00083"></a>00083 <span class="comment"> * \f[ COV = H M H^\top \f]</span>
<a name="l00084"></a>00084 <span class="comment"> * With COV the covariance matrix of the optimal parameters, H this matrix, and M the covariance of the input (observations).</span>
<a name="l00085"></a>00085 <span class="comment"> */</span>
<a name="l00086"></a>00086 <span class="comment">//CMatrixTemplateNumeric<NUMTYPE> H;</span>
<a name="l00087"></a>00087 };
<a name="l00088"></a>00088 <span class="comment"></span>
<a name="l00089"></a>00089 <span class="comment"> /** Executes the LM-method, with derivatives estimated from</span>
<a name="l00090"></a>00090 <span class="comment"> * \a functor is a user-provided function which takes as input two vectors, in this order:</span>
<a name="l00091"></a>00091 <span class="comment"> * - x: The parameters to be optimized.</span>
<a name="l00092"></a>00092 <span class="comment"> * - userParam: The vector passed to the LM algorithm, unmodified.</span>
<a name="l00093"></a>00093 <span class="comment"> * and must return the "error vector", or the error (not squared) in each measured dimension, so the sum of the square of that output is the overall square error.</span>
<a name="l00094"></a>00094 <span class="comment"> *</span>
<a name="l00095"></a>00095 <span class="comment"> * \a x_increment_adder Is an optional functor which may replace the Euclidean "x_new = x + x_increment" at the core of the incremental optimizer by</span>
<a name="l00096"></a>00096 <span class="comment"> * any other operation. It can be used for example, in on-manifold optimizations.</span>
<a name="l00097"></a>00097 <span class="comment"> */</span>
<a name="l00098"></a><a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#a0d648bdc68df4a0fcd5d4e3438a0c3b2">00098</a> <span class="keyword">static</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#a0d648bdc68df4a0fcd5d4e3438a0c3b2" title="Executes the LM-method, with derivatives estimated from functor is a user-provided function which tak...">execute</a>(
<a name="l00099"></a>00099 VECTORTYPE &out_optimal_x,
<a name="l00100"></a>00100 <span class="keyword">const</span> VECTORTYPE &x0,
<a name="l00101"></a>00101 <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#af1417a100aac50f016fcbd86455172fd" title="The type of the function passed to execute.">TFunctorEval</a> functor,
<a name="l00102"></a>00102 <span class="keyword">const</span> VECTORTYPE &increments,
<a name="l00103"></a>00103 <span class="keyword">const</span> USERPARAM &userParam,
<a name="l00104"></a>00104 <a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html">TResultInfo</a> &out_info,
<a name="l00105"></a>00105 <span class="keywordtype">bool</span> verbose = <span class="keyword">false</span>,
<a name="l00106"></a>00106 <span class="keyword">const</span> <span class="keywordtype">size_t</span> maxIter = 200,
<a name="l00107"></a>00107 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> tau = 1e-3,
<a name="l00108"></a>00108 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> e1 = 1e-8,
<a name="l00109"></a>00109 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> e2 = 1e-8,
<a name="l00110"></a>00110 <span class="keywordtype">bool</span> returnPath =<span class="keyword">true</span>,
<a name="l00111"></a>00111 <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#ac116d455d54d9d260f78be3264c1cf0e" title="The type of an optional functor passed to execute to replace the Euclidean addition "x_new = x_old + ...">TFunctorIncrement</a> x_increment_adder = NULL
<a name="l00112"></a>00112 )
<a name="l00113"></a>00113 {
<a name="l00114"></a>00114 <span class="keyword">using namespace </span>mrpt;
<a name="l00115"></a>00115 <span class="keyword">using namespace </span>mrpt::utils;
<a name="l00116"></a>00116 <span class="keyword">using namespace </span>mrpt::math;
<a name="l00117"></a>00117 <span class="keyword">using namespace </span>std;
<a name="l00118"></a>00118
<a name="l00119"></a>00119 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00120"></a>00120
<a name="l00121"></a>00121 VECTORTYPE &x=out_optimal_x; <span class="comment">// Var rename</span>
<a name="l00122"></a>00122
<a name="l00123"></a>00123 <span class="comment">// Asserts:</span>
<a name="l00124"></a>00124 <a class="code" href="mrpt__macros_8h.html#a47eb5a445c2bf3d9190396510ea9683e">ASSERT_</a>( increments.size() == x0.size() );
<a name="l00125"></a>00125
<a name="l00126"></a>00126 x=x0; <span class="comment">// Start with the starting point</span>
<a name="l00127"></a>00127 VECTORTYPE f_x; <span class="comment">// The vector error from the user function</span>
<a name="l00128"></a>00128 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<NUMTYPE></a> AUX;
<a name="l00129"></a>00129 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<NUMTYPE></a> J; <span class="comment">// The Jacobian of "f"</span>
<a name="l00130"></a>00130 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html">CMatrixTemplateNumeric<NUMTYPE></a> H; <span class="comment">// The Hessian of "f"</span>
<a name="l00131"></a>00131 VECTORTYPE g; <span class="comment">// The gradient</span>
<a name="l00132"></a>00132
<a name="l00133"></a>00133 <span class="comment">// Compute the jacobian and the Hessian:</span>
<a name="l00134"></a>00134 <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">mrpt::math::estimateJacobian</a>( x, functor, increments, userParam, J);
<a name="l00135"></a>00135 H.multiply_AtA(J);
<a name="l00136"></a>00136
<a name="l00137"></a>00137 <span class="keyword">const</span> <span class="keywordtype">size_t</span> H_len = H.getColCount();
<a name="l00138"></a>00138
<a name="l00139"></a>00139 <span class="comment">// Compute the gradient:</span>
<a name="l00140"></a>00140 functor(x, userParam ,f_x);
<a name="l00141"></a>00141 J.multiply_Atb(f_x, g);
<a name="l00142"></a>00142
<a name="l00143"></a>00143 <span class="comment">// Start iterations:</span>
<a name="l00144"></a>00144 <span class="keywordtype">bool</span> found = <a class="code" href="namespacemrpt_1_1math.html#af104d8452be90bb99aa78ef15168c37a">math::norm_inf</a>(g)<=e1;
<a name="l00145"></a>00145 <span class="keywordflow">if</span> (verbose && found) <a class="code" href="classmrpt_1_1utils_1_1_c_debug_output_capable.html#ab78281b5d70d6e295a8527a10fea66de" title="Sends a formated text to "debugOut" if not NULL, or to cout otherwise.">printf_debug</a>(<span class="stringliteral">"[LM] End condition: math::norm_inf(g)<=e1 :%f\n"</span>,<a class="code" href="namespacemrpt_1_1math.html#af104d8452be90bb99aa78ef15168c37a">math::norm_inf</a>(g));
<a name="l00146"></a>00146
<a name="l00147"></a>00147 <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> lambda = tau * H.maximumDiagonal();
<a name="l00148"></a>00148 <span class="keywordtype">size_t</span> iter = 0;
<a name="l00149"></a>00149 <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> v = 2;
<a name="l00150"></a>00150
<a name="l00151"></a>00151 VECTORTYPE h_lm;
<a name="l00152"></a>00152 VECTORTYPE xnew, f_xnew ;
<a name="l00153"></a>00153 <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a> F_x = pow( <a class="code" href="namespacemrpt_1_1math.html#a5cb3c01d8c6c1577d5e93daba2cfe62a">math::norm</a>( f_x ), 2);
<a name="l00154"></a>00154
<a name="l00155"></a>00155 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = x.size();
<a name="l00156"></a>00156
<a name="l00157"></a>00157 <span class="keywordflow">if</span> (returnPath) {
<a name="l00158"></a>00158 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>.setSize(maxIter,N+1);
<a name="l00159"></a>00159 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>.block(iter,0,1,N) = x.transpose();
<a name="l00160"></a>00160 } <span class="keywordflow">else</span> out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a> = Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>(); <span class="comment">// Empty matrix</span>
<a name="l00161"></a>00161
<a name="l00162"></a>00162 <span class="keywordflow">while</span> (!found && ++iter<maxIter)
<a name="l00163"></a>00163 {
<a name="l00164"></a>00164 <span class="comment">// H_lm = -( H + \lambda I ) ^-1 * g</span>
<a name="l00165"></a>00165 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> k=0;k<H_len;k++)
<a name="l00166"></a>00166 H(k,k)+= lambda;
<a name="l00167"></a>00167 <span class="comment">//H(k,k) *= 1+lambda;</span>
<a name="l00168"></a>00168
<a name="l00169"></a>00169 H.inv_fast(AUX);
<a name="l00170"></a>00170 AUX.multiply_Ab(g,h_lm);
<a name="l00171"></a>00171 h_lm *= <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html#aa1a845f3fb7ddebb03b619511751fc9b">NUMTYPE</a>(-1.0);
<a name="l00172"></a>00172
<a name="l00173"></a>00173 <span class="keywordtype">double</span> h_lm_n2 = <a class="code" href="namespacemrpt_1_1math.html#a5cb3c01d8c6c1577d5e93daba2cfe62a">math::norm</a>(h_lm);
<a name="l00174"></a>00174 <span class="keywordtype">double</span> x_n2 = <a class="code" href="namespacemrpt_1_1math.html#a5cb3c01d8c6c1577d5e93daba2cfe62a">math::norm</a>(x);
<a name="l00175"></a>00175
<a name="l00176"></a>00176 <span class="keywordflow">if</span> (verbose) <a class="code" href="classmrpt_1_1utils_1_1_c_debug_output_capable.html#ab78281b5d70d6e295a8527a10fea66de" title="Sends a formated text to "debugOut" if not NULL, or to cout otherwise.">printf_debug</a>( (<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"[LM] Iter: %u x:"</span>,(<span class="keywordtype">unsigned</span>)iter)+ <a class="code" href="group__stlext__grp.html#ga39a976e64941bfbbcde6969124bdd6c5" title="Generates a string for a vector in the format [A,B,C,...] to std::cout, and the fmt string for each v...">sprintf_vector</a>(<span class="stringliteral">" %f"</span>,x) + <span class="keywordtype">string</span>(<span class="stringliteral">"\n"</span>)).c_str() );
<a name="l00177"></a>00177
<a name="l00178"></a>00178 <span class="keywordflow">if</span> (h_lm_n2<e2*(x_n2+e2))
<a name="l00179"></a>00179 {
<a name="l00180"></a>00180 <span class="comment">// Done:</span>
<a name="l00181"></a>00181 found = <span class="keyword">true</span>;
<a name="l00182"></a>00182 <span class="keywordflow">if</span> (verbose) <a class="code" href="classmrpt_1_1utils_1_1_c_debug_output_capable.html#ab78281b5d70d6e295a8527a10fea66de" title="Sends a formated text to "debugOut" if not NULL, or to cout otherwise.">printf_debug</a>(<span class="stringliteral">"[LM] End condition: %e < %e\n"</span>, h_lm_n2, e2*(x_n2+e2) );
<a name="l00183"></a>00183 }
<a name="l00184"></a>00184 <span class="keywordflow">else</span>
<a name="l00185"></a>00185 {
<a name="l00186"></a>00186 <span class="comment">// Improvement: xnew = x + h_lm;</span>
<a name="l00187"></a>00187 <span class="keywordflow">if</span> (!x_increment_adder)
<a name="l00188"></a>00188 xnew = x + h_lm; <span class="comment">// Normal Euclidean space addition.</span>
<a name="l00189"></a>00189 <span class="keywordflow">else</span> x_increment_adder(xnew, x, h_lm, userParam);
<a name="l00190"></a>00190
<a name="l00191"></a>00191 functor(xnew, userParam ,f_xnew );
<a name="l00192"></a>00192 <span class="keyword">const</span> <span class="keywordtype">double</span> F_xnew = pow( <a class="code" href="namespacemrpt_1_1math.html#a5cb3c01d8c6c1577d5e93daba2cfe62a">math::norm</a>(f_xnew), 2);
<a name="l00193"></a>00193
<a name="l00194"></a>00194 <span class="comment">// denom = h_lm^t * ( \lambda * h_lm - g )</span>
<a name="l00195"></a>00195 VECTORTYPE tmp(h_lm);
<a name="l00196"></a>00196 tmp *= lambda;
<a name="l00197"></a>00197 tmp -= g;
<a name="l00198"></a>00198 tmp.array() *=h_lm.array();
<a name="l00199"></a>00199 <span class="keywordtype">double</span> denom = tmp.sum();
<a name="l00200"></a>00200 <span class="keywordtype">double</span> l = (F_x - F_xnew) / denom;
<a name="l00201"></a>00201
<a name="l00202"></a>00202 <span class="keywordflow">if</span> (l>0) <span class="comment">// There is an improvement:</span>
<a name="l00203"></a>00203 {
<a name="l00204"></a>00204 <span class="comment">// Accept new point:</span>
<a name="l00205"></a>00205 x = xnew;
<a name="l00206"></a>00206 f_x = f_xnew;
<a name="l00207"></a>00207 F_x = F_xnew;
<a name="l00208"></a>00208
<a name="l00209"></a>00209 <a class="code" href="group__container__ops__grp.html#ga13e2e339d944b37f2386fc4e0bb56935" title="Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...">math::estimateJacobian</a>( x, functor, increments, userParam, J);
<a name="l00210"></a>00210 H.multiply_AtA(J);
<a name="l00211"></a>00211 J.multiply_Atb(f_x, g);
<a name="l00212"></a>00212
<a name="l00213"></a>00213 found = <a class="code" href="namespacemrpt_1_1math.html#af104d8452be90bb99aa78ef15168c37a">math::norm_inf</a>(g)<=e1;
<a name="l00214"></a>00214 <span class="keywordflow">if</span> (verbose && found) <a class="code" href="classmrpt_1_1utils_1_1_c_debug_output_capable.html#ab78281b5d70d6e295a8527a10fea66de" title="Sends a formated text to "debugOut" if not NULL, or to cout otherwise.">printf_debug</a>(<span class="stringliteral">"[LM] End condition: math::norm_inf(g)<=e1 : %e\n"</span>, <a class="code" href="namespacemrpt_1_1math.html#af104d8452be90bb99aa78ef15168c37a">math::norm_inf</a>(g) );
<a name="l00215"></a>00215
<a name="l00216"></a>00216 lambda *= max(0.33, 1-pow(2*l-1,3) );
<a name="l00217"></a>00217 v = 2;
<a name="l00218"></a>00218 }
<a name="l00219"></a>00219 <span class="keywordflow">else</span>
<a name="l00220"></a>00220 {
<a name="l00221"></a>00221 <span class="comment">// Nope...</span>
<a name="l00222"></a>00222 lambda *= v;
<a name="l00223"></a>00223 v*= 2;
<a name="l00224"></a>00224 }
<a name="l00225"></a>00225
<a name="l00226"></a>00226
<a name="l00227"></a>00227 <span class="keywordflow">if</span> (returnPath) {
<a name="l00228"></a>00228 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>.block(iter,0,1,x.size()) = x.transpose();
<a name="l00229"></a>00229 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>(iter,x.size()) = F_x;
<a name="l00230"></a>00230 }
<a name="l00231"></a>00231 }
<a name="l00232"></a>00232 } <span class="comment">// end while</span>
<a name="l00233"></a>00233
<a name="l00234"></a>00234 <span class="comment">// Output info:</span>
<a name="l00235"></a>00235 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#accb179389ffd5bde573e798e73b9b9bf">final_sqr_err</a> = F_x;
<a name="l00236"></a>00236 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2e93e60501a6978c38b2204593b3cc66">iterations_executed</a> = iter;
<a name="l00237"></a>00237 out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a8721a85def6d8560eb69789066d70743" title="The last error vector returned by the user-provided functor.">last_err_vector</a> = f_x;
<a name="l00238"></a>00238 <span class="keywordflow">if</span> (returnPath) out_info.<a class="code" href="structmrpt_1_1math_1_1_c_levenberg_marquardt_templ_1_1_t_result_info.html#a2c0fb070d18fd9a0ecdd99d3def871aa" title="Each row is the optimized value at each iteration.">path</a>.setSize(iter,N+1);
<a name="l00239"></a>00239
<a name="l00240"></a>00240 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00241"></a>00241 }
<a name="l00242"></a>00242
<a name="l00243"></a>00243 }; <span class="comment">// End of class def.</span>
<a name="l00244"></a>00244
<a name="l00245"></a>00245
<a name="l00246"></a><a class="code" href="namespacemrpt_1_1math.html#a527b33ac3ec062cf2c680515086dd9d9">00246</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_c_levenberg_marquardt_templ.html" title="An implementation of the Levenberg-Marquardt algorithm for least-square minimization.">CLevenbergMarquardtTempl<vector_double></a> <a class="code" href="namespacemrpt_1_1math.html#a527b33ac3ec062cf2c680515086dd9d9" title="The default name for the LM class is an instantiation for "double".">CLevenbergMarquardt</a>; <span class="comment">//!< The default name for the LM class is an instantiation for "double"</span>
<a name="l00247"></a>00247 <span class="comment"></span>
<a name="l00248"></a>00248 } <span class="comment">// End of namespace</span>
<a name="l00249"></a>00249 } <span class="comment">// End of namespace</span>
<a name="l00250"></a>00250 <span class="preprocessor">#endif</span>
</pre></div></div>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
