MRPT  1.9.9
LevMarqTest_impl.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
11 #include <mrpt/system/CTicTac.h>
12 #include <cmath>
13 #include <iostream>
14 
15 using namespace mrpt;
16 using namespace mrpt::math;
17 using namespace mrpt::system;
18 using namespace std;
19 
20 double levmarq_final_error = 1e10;
21 
22 // The error function F(x):
24  const CVectorDouble& x, const CVectorDouble& y, CVectorDouble& out_f)
25 {
26  out_f.resize(1);
27  // 1-cos(x+1) *cos(x*y+1)
28  out_f[0] = 1 - cos(x[0] + 1) * cos(x[0] * x[1] + 1);
29 }
30 
32 {
33  CVectorDouble optimal_x;
34  CVectorDouble initial_x;
35  CVectorDouble y;
36 
38  CTicTac tictac;
39 
40  initial_x.resize(2);
41  initial_x[0] = 1.4; // x
42  initial_x[1] = 2.5; // y
43 
44  CVectorDouble increments_x(2);
45  increments_x.fill(0.0001);
46 
47  size_t N = 1;
48 
50  tictac.Tic();
51  for (size_t k = 0; k < N; k++)
52  lm.execute(optimal_x, initial_x, myFunction, increments_x, y, info);
53 
54  levmarq_final_error = std::sqrt(info.final_sqr_err);
55 
56 #ifdef LEVMARQ_EXAMPLE_VERBOSE
57  const auto T = tictac.Tac() / N;
58  cout << "Iterations: " << info.iterations_executed << endl;
59  cout << "Final sqr error: " << info.final_sqr_err << endl;
60 
61  cout << endl << "Final optimized position: " << optimal_x << endl;
62 
63  cout << "Time: " << T * 1e6 << " us" << endl;
64 
65  info.path.saveToTextFile("lm-path.txt");
66  cout << "Path saved to 'lm-path.txt'" << endl;
67 #endif
68 }
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
void execute(VECTORTYPE &out_optimal_x, const VECTORTYPE &x0, TFunctorEval functor, const VECTORTYPE &increments, const USERPARAM &userParam, TResultInfo &out_info, mrpt::system::VerbosityLevel verbosity=mrpt::system::LVL_INFO, const size_t maxIter=200, const NUMTYPE tau=1e-3, const NUMTYPE e1=1e-8, const NUMTYPE e2=1e-8, bool returnPath=true, TFunctorIncrement x_increment_adder=nullptr)
Executes the LM-method, with derivatives estimated from functor is a user-provided function which tak...
void fill(const Scalar &val)
A high-performance stopwatch, with typical resolution of nanoseconds.
STL namespace.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
This base provides a set of functions for maths stuff.
matrix_t path
Each row is the optimized value at each iteration.
void myFunction(const CVectorDouble &x, const CVectorDouble &y, CVectorDouble &out_f)
void TestLevMarq()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double levmarq_final_error
void resize(std::size_t N, bool zeroNewElements=false)
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020