00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CLevenbergMarquardt_H
00029 #define CLevenbergMarquardt_H
00030
00031 #include <mrpt/utils/CDebugOutputCapable.h>
00032 #include <mrpt/math/CMatrixD.h>
00033 #include <mrpt/math/utils.h>
00034
00035
00036
00037
00038 namespace mrpt
00039 {
00040 namespace math
00041 {
00042
00043
00044
00045
00046
00047
00048
00049 template <typename VECTORTYPE = mrpt::vector_double, class USERPARAM = VECTORTYPE >
00050 class CLevenbergMarquardtTempl : public mrpt::utils::CDebugOutputCapable
00051 {
00052 public:
00053 typedef typename VECTORTYPE::value_type NUMTYPE;
00054
00055
00056
00057
00058
00059
00060
00061 typedef void (*TFunctor)(
00062 const VECTORTYPE &x,
00063 const USERPARAM &y,
00064 VECTORTYPE &out);
00065
00066 struct TResultInfo
00067 {
00068 NUMTYPE final_sqr_err;
00069 size_t iterations_executed;
00070 VECTORTYPE last_err_vector;
00071 CMatrixTemplateNumeric<NUMTYPE> path;
00072
00073
00074
00075
00076
00077
00078 };
00079
00080
00081
00082
00083
00084
00085
00086 static void execute(
00087 VECTORTYPE &out_optimal_x,
00088 const VECTORTYPE &x0,
00089 TFunctor functor,
00090 const VECTORTYPE &increments,
00091 const USERPARAM &userParam,
00092 TResultInfo &out_info,
00093 bool verbose = false,
00094 const size_t &maxIter = 200,
00095 const NUMTYPE tau = 1e-3,
00096 const NUMTYPE e1 = 1e-8,
00097 const NUMTYPE e2 = 1e-8,
00098 bool returnPath=true
00099 )
00100 {
00101 using namespace mrpt;
00102 using namespace mrpt::utils;
00103 using namespace mrpt::math;
00104 using namespace std;
00105
00106 MRPT_START;
00107
00108 VECTORTYPE &x=out_optimal_x;
00109
00110
00111 ASSERT_( increments.size() == x0.size() );
00112
00113 x=x0;
00114 VECTORTYPE f_x;
00115 CMatrixTemplateNumeric<NUMTYPE> AUX;
00116 CMatrixTemplateNumeric<NUMTYPE> J;
00117 CMatrixTemplateNumeric<NUMTYPE> H;
00118 VECTORTYPE g;
00119
00120
00121 mrpt::math::estimateJacobian( x, functor, increments, userParam, J);
00122 H.multiply_AtA(J);
00123
00124 const size_t H_len = H.getColCount();
00125
00126
00127 functor(x, userParam ,f_x);
00128 J.multiply_Atb(f_x, g);
00129
00130
00131 bool found = math::norm_inf(g)<=e1;
00132 if (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 :%f\n",math::norm_inf(g));
00133
00134 NUMTYPE lambda = tau * H.maximumDiagonal();
00135 size_t iter = 0;
00136 NUMTYPE v = 2;
00137
00138 VECTORTYPE h_lm;
00139 VECTORTYPE xnew, f_xnew ;
00140 NUMTYPE F_x = pow( math::norm( f_x ), 2);
00141
00142 const size_t N = x.size();
00143
00144 if (returnPath) {
00145 out_info.path.setSize(maxIter,N+1);
00146 out_info.path.block(iter,0,1,N) = x.transpose();
00147 } else out_info.path = Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>();
00148
00149 while (!found && ++iter<maxIter)
00150 {
00151
00152 for (size_t k=0;k<H_len;k++)
00153 H(k,k)+= lambda;
00154
00155
00156 H.inv_fast(AUX);
00157 AUX.multiply_Ab(g,h_lm);
00158 h_lm *= NUMTYPE(-1.0);
00159
00160 double h_lm_n2 = math::norm(h_lm);
00161 double x_n2 = math::norm(x);
00162
00163 if (verbose) printf_debug( (format("[LM] Iter: %u x:",(unsigned)iter)+ sprintf_vector(" %f",x) + string("\n")).c_str() );
00164
00165 if (h_lm_n2<e2*(x_n2+e2))
00166 {
00167
00168 found = true;
00169 if (verbose) printf_debug("[LM] End condition: %e < %e\n", h_lm_n2, e2*(x_n2+e2) );
00170 }
00171 else
00172 {
00173
00174 xnew = x;
00175 xnew += h_lm;
00176 functor(xnew, userParam ,f_xnew );
00177 const double F_xnew = pow( math::norm(f_xnew), 2);
00178
00179
00180 VECTORTYPE tmp(h_lm);
00181 tmp *= lambda;
00182 tmp -= g;
00183 tmp.array() *=h_lm.array();
00184 double denom = tmp.sum();
00185 double l = (F_x - F_xnew) / denom;
00186
00187 if (l>0)
00188 {
00189
00190 x = xnew;
00191 f_x = f_xnew;
00192 F_x = F_xnew;
00193
00194 math::estimateJacobian( x, functor, increments, userParam, J);
00195 H.multiply_AtA(J);
00196 J.multiply_Atb(f_x, g);
00197
00198 found = math::norm_inf(g)<=e1;
00199 if (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 : %e\n", math::norm_inf(g) );
00200
00201 lambda *= max(0.33, 1-pow(2*l-1,3) );
00202 v = 2;
00203 }
00204 else
00205 {
00206
00207 lambda *= v;
00208 v*= 2;
00209 }
00210
00211
00212 if (returnPath) {
00213 out_info.path.block(iter,0,1,x.size()) = x.transpose();
00214 out_info.path(iter,x.size()) = F_x;
00215 }
00216 }
00217 }
00218
00219
00220 out_info.final_sqr_err = F_x;
00221 out_info.iterations_executed = iter;
00222 out_info.last_err_vector = f_x;
00223 if (returnPath) out_info.path.setSize(iter,N+1);
00224
00225 MRPT_END;
00226 }
00227
00228 };
00229
00230
00231 typedef CLevenbergMarquardtTempl<vector_double> CLevenbergMarquardt;
00232
00233 }
00234 }
00235 #endif