SourceXtractorPlusPlus  0.16
Please provide a description of the project.
LevmarEngine.cpp
Go to the documentation of this file.
1 
23 #include <cmath>
24 #include <mutex>
25 
26 #include <levmar.h>
28 #include <ElementsKernel/Logging.h>
31 
32 #ifndef LEVMAR_WORKAREA_MAX_SIZE
33 #define LEVMAR_WORKAREA_MAX_SIZE size_t(2ul<<30) // 2 GiB
34 #endif
35 
36 namespace {
37 
38 __attribute__((unused))
39 void printLevmarInfo(std::array<double, 10> info) {
40  std::cerr << "\nMinimization info:\n";
41  std::cerr << " ||e||_2 at initial p: " << info[0] << '\n';
42  std::cerr << " ||e||_2: " << info[1] << '\n';
43  std::cerr << " ||J^T e||_inf: " << info[2] << '\n';
44  std::cerr << " ||Dp||_2: " << info[3] << '\n';
45  std::cerr << " mu/max[J^T J]_ii: " << info[4] << '\n';
46  std::cerr << " # iterations: " << info[5] << '\n';
47  switch ((int) info[6]) {
48  case 1:
49  std::cerr << " stopped by small gradient J^T e\n";
50  break;
51  case 2:
52  std::cerr << " stopped by small Dp\n";
53  break;
54  case 3:
55  std::cerr << " stopped by itmax\n";
56  break;
57  case 4:
58  std::cerr << " singular matrix. Restart from current p with increased mu\n";
59  break;
60  case 5:
61  std::cerr << " no further error reduction is possible. Restart with increased mu\n";
62  break;
63  case 6:
64  std::cerr << " stopped by small ||e||_2\n";
65  break;
66  case 7:
67  std::cerr << " stopped by invalid (i.e. NaN or Inf) func values; a user error\n";
68  break;
69  }
70  std::cerr << " # function evaluations: " << info[7] << '\n';
71  std::cerr << " # Jacobian evaluations: " << info[8] << '\n';
72  std::cerr << " # linear systems solved: " << info[9] << "\n\n";
73 }
74 
75 }
76 
77 namespace ModelFitting {
78 
80 
81 static std::shared_ptr<LeastSquareEngine> createLevmarEngine(unsigned max_iterations) {
82  return std::make_shared<LevmarEngine>(max_iterations);
83 }
84 
86 
88  if (res == -1) {
90  }
91  switch (static_cast<int>(info[6])) {
92  case 7:
93  case 4:
95  case 3:
97  default:
99  }
100 }
101 
102 LevmarEngine::LevmarEngine(size_t itmax, double tau, double epsilon1,
103  double epsilon2, double epsilon3, double delta)
104  : m_itmax{itmax}, m_opts{tau, epsilon1, epsilon2, epsilon3, delta} {
105 #ifdef LINSOLVERS_RETAIN_MEMORY
106  logger.warn() << "Using a non thread safe levmar! Parallelism will be reduced.";
107 #endif
108 }
109 
110 LevmarEngine::~LevmarEngine() = default;
111 
112 
113 #ifdef LINSOLVERS_RETAIN_MEMORY
114 // If the Levmar library is not configured for multithreading, this mutex is used to ensure only one thread
115 // at a time can enter levmar
116 namespace {
117  std::mutex levmar_mutex;
118 }
119 #endif
120 
122  ResidualEstimator& residual_estimator) {
123  // Create a tuple which keeps the references to the given manager and estimator
124  auto adata = std::tie(parameter_manager, residual_estimator);
125 
126  // The function which is called by the levmar loop
127  auto levmar_res_func = [](double *p, double *hx, int, int, void *extra) {
128 #ifdef LINSOLVERS_RETAIN_MEMORY
129  levmar_mutex.unlock();
130 #endif
131 
132  auto* extra_ptr = (decltype(adata)*)extra;
133  EngineParameterManager& pm = std::get<0>(*extra_ptr);
134  pm.updateEngineValues(p);
135  ResidualEstimator& re = std::get<1>(*extra_ptr);
136  re.populateResiduals(hx);
137 
138 #ifdef LINSOLVERS_RETAIN_MEMORY
139  levmar_mutex.lock();
140 #endif
141  };
142 
143  // Create the vector which will be used for keeping the parameter values
144  // and initialize it to the current values of the parameters
145  std::vector<double> param_values (parameter_manager.numberOfParameters());
146  parameter_manager.getEngineValues(param_values.begin());
147 
148  // Create a vector for getting the information of the minimization
150 
151  std::vector<double> covariance_matrix (parameter_manager.numberOfParameters() * parameter_manager.numberOfParameters());
152 
153 #ifdef LINSOLVERS_RETAIN_MEMORY
154  levmar_mutex.lock();
155 #endif
156 
157  std::unique_ptr<double[]> workarea;
158  size_t workarea_size = LM_DIF_WORKSZ(parameter_manager.numberOfParameters(),
159  residual_estimator.numberOfResiduals());
160 
161  if (workarea_size <= LEVMAR_WORKAREA_MAX_SIZE / sizeof(double)) {
162  try {
163  workarea.reset(new double[workarea_size]);
164  } catch (const std::bad_alloc&) {
165  }
166  }
167 
168  // Didn't allocate
169  if (workarea == nullptr) {
170  LeastSquareSummary summary {};
172  summary.iteration_no = workarea_size;
173  summary.parameter_sigmas.resize(parameter_manager.numberOfParameters());
174  return summary;
175  }
176 
177  // Call the levmar library
178  auto start = std::chrono::steady_clock::now();
179  auto res = dlevmar_dif(levmar_res_func, // The function called from the levmar algorithm
180  param_values.data(), // The pointer where the parameter values are
181  NULL, // We don't use any measurement vector
182  parameter_manager.numberOfParameters(), // The number of free parameters
183  residual_estimator.numberOfResiduals(), // The number of residuals
184  m_itmax, // The maximum number of iterations
185  m_opts.data(), // The minimization options
186  info.data(), // Where the information of the minimization is stored
187  workarea.get(), // Working memory is allocated internally
188  covariance_matrix.data(),
189  &adata // No additional data needed
190  );
192  std::chrono::duration<float> elapsed = end - start;
193 #ifdef LINSOLVERS_RETAIN_MEMORY
194  levmar_mutex.unlock();
195 #endif
196 
197  // Create and return the summary object
198  LeastSquareSummary summary {};
199 
200  auto converted_covariance_matrix = parameter_manager.convertCovarianceMatrixToWorldSpace(covariance_matrix);
201  for (unsigned int i=0; i<parameter_manager.numberOfParameters(); i++) {
202  summary.parameter_sigmas.push_back(sqrt(converted_covariance_matrix[i*(parameter_manager.numberOfParameters()+1)]));
203  }
204 
205  summary.status_flag = getStatusFlag(info, res);
206  summary.engine_stop_reason = info[6];
207  summary.iteration_no = info[5];
208  summary.underlying_framework_info = info;
209  summary.duration = elapsed.count();
210  return summary;
211 }
212 
213 } // end of namespace ModelFitting
#define LEVMAR_WORKAREA_MAX_SIZE
T begin(T... args)
static Logging getLogger(const std::string &name="")
void warn(const std::string &logMessage)
Class responsible for managing the parameters the least square engine minimizes.
void updateEngineValues(DoubleIter new_values_iter)
Updates the managed parameters with the given engine values.
std::vector< double > convertCovarianceMatrixToWorldSpace(std::vector< double > covariance_matrix) const
std::size_t numberOfParameters()
Returns the number of parameters managed by the manager.
void getEngineValues(DoubleIter output_iter) const
Returns the engine values of the managed parameters.
LeastSquareSummary solveProblem(EngineParameterManager &parameter_manager, ResidualEstimator &residual_estimator) override
virtual ~LevmarEngine()
Destructor.
std::vector< double > m_opts
Definition: LevmarEngine.h:72
LevmarEngine(size_t itmax=1000, double tau=1E-3, double epsilon1=1E-8, double epsilon2=1E-8, double epsilon3=1E-8, double delta=1E-4)
Constructs a new instance of the engine.
Provides to the LeastSquareEngine the residual values.
void populateResiduals(DoubleIter output_iter) const
T data(T... args)
T end(T... args)
T get(T... args)
#define __attribute__(x)
static LeastSquareSummary::StatusFlag getStatusFlag(int ret)
Definition: GSLEngine.cpp:106
static std::shared_ptr< LeastSquareEngine > createLevmarEngine(unsigned max_iterations)
static LeastSquareEngineManager::StaticEngine levmar_engine
static Elements::Logging logger
T push_back(T... args)
T reset(T... args)
T sqrt(T... args)
Class containing the summary information of solving a least square minimization problem.
StatusFlag status_flag
Flag indicating if the minimization was successful.
T tie(T... args)