LevMarFitting.h
Go to the documentation of this file.
3 #include <algorithm>
4 #include <iostream>
5 #include "LevMarFunc.h"
6 #ifdef DOPARALLEL
7 #include <omp.h>
8 #endif
9
10 template< class ScalarT >
11 bool Cholesky(ScalarT* a, size_t n, ScalarT p[])
12 /*Given a positive-definite symmetric matrix a[1..n][1..n], this routine constructs its Cholesky
13 decomposition, A = L · LT . On input, only the upper triangle of a need be given; it is not
14 modified. The Cholesky factor L is returned in the lower triangle of a, except for its diagonal
15 elements which are returned in p[1..n].*/
16 {
17  size_t i, j, k;
18  ScalarT sum;
19  for (i = 0; i < n; ++i)
20  {
21  for (j = i; j < n; ++j)
22  {
23  for (sum = a[i * n + j], k = i - 1; k != -1; --k)
24  {
25  sum -= a[i * n + k] * a[j * n + k];
26  }
27  if (i == j)
28  {
29  if (sum <= ScalarT(0))
30  // a, with rounding errors, is not positive definite.
31  {
32  return false;
33  }
34  p[i] = std::sqrt(sum);
35  }
36  else
37  {
38  a[j * n + i] = sum / p[i];
39  }
40  }
41  }
42  return true;
43 }
44
45 template< class ScalarT, unsigned int N >
46 bool Cholesky(ScalarT* a, ScalarT p[])
47 /*Given a positive-definite symmetric matrix a[1..n][1..n], this routine constructs its Cholesky
48 decomposition, A = L · LT . On input, only the upper triangle of a need be given; it is not
49 modified. The Cholesky factor L is returned in the lower triangle of a, except for its diagonal
50 elements which are returned in p[1..n].*/
51 {
52  size_t i, j, k;
53  ScalarT sum;
54  for (i = 0; i < N; ++i)
55  {
56  for (j = i; j < N; ++j)
57  {
58  for (sum = a[i * N + j], k = i - 1; k != -1; --k)
59  {
60  sum -= a[i * N + k] * a[j * N + k];
61  }
62  if (i == j)
63  {
64  if (sum <= ScalarT(0))
65  // a, with rounding errors, is not positive definite.
66  {
67  return false;
68  }
69  p[i] = std::sqrt(sum);
70  }
71  else
72  {
73  a[j * N + i] = sum / p[i];
74  }
75  }
76  }
77  return true;
78 }
79
80 template< class ScalarT >
81 void CholeskySolve(ScalarT* a, size_t n, ScalarT p[], ScalarT b[], ScalarT x[])
82 /*Solves the set of n linear equations A · x = b, where a is a positive-definite symmetric matrix.
83 a[1..n][1..n] and p[1..n] are input as the output of the routine choldc. Only the lower
84 subdiagonal portion of a is accessed. b[1..n] is input as the right-hand side vector. The
85 solution vector is returned in x[1..n]. a, n, and p are not modified and can be left in place
86 for successive calls with different right-hand sides b. b is not modified unless you identify b and
87 x in the calling sequence, which is allowed.*/
88 {
89  size_t i, k;
90  ScalarT sum;
91  for (i = 0; i < n; i++)
92  {
93  // Solve L · y = b, storing y in x.
94  for (sum = b[i], k = i - 1; k != -1; --k)
95  {
96  sum -= a[i * n + k] * x[k];
97  }
98  x[i] = sum / p[i];
99  }
100  for (i = n - 1; i != -1; --i)
101  {
102  // Solve LT · x = y.
103  for (sum = x[i], k = i + 1; k < n; ++k)
104  {
105  sum -= a[k * n + i] * x[k];
106  }
107  x[i] = sum / p[i];
108  }
109 }
110
111 template< class ScalarT, unsigned int N >
112 void CholeskySolve(ScalarT* a, ScalarT p[], ScalarT b[], ScalarT x[])
113 /*Solves the set of n linear equations A · x = b, where a is a positive-definite symmetric matrix.
114 a[1..n][1..n] and p[1..n] are input as the output of the routine choldc. Only the lower
115 subdiagonal portion of a is accessed. b[1..n] is input as the right-hand side vector. The
116 solution vector is returned in x[1..n]. a, n, and p are not modified and can be left in place
117 for successive calls with different right-hand sides b. b is not modified unless you identify b and
118 x in the calling sequence, which is allowed.*/
119 {
120  size_t i, k;
121  ScalarT sum;
122  for (i = 0; i < N; i++)
123  {
124  // Solve L · y = b, storing y in x.
125  for (sum = b[i], k = i - 1; k != -1; --k)
126  {
127  sum -= a[i * N + k] * x[k];
128  }
129  x[i] = sum / p[i];
130  }
131  for (i = N - 1; i != -1; --i)
132  {
133  // Solve LT · x = y.
134  for (sum = x[i], k = i + 1; k < N; ++k)
135  {
136  sum -= a[k * N + i] * x[k];
137  }
138  x[i] = sum / p[i];
139  }
140 }
141
142 template< class IteratorT, class FuncT >
143 bool LevMar(IteratorT begin, IteratorT end, FuncT& func,
144  typename FuncT::ScalarType* param)
145 {
146  typedef typename FuncT::ScalarType ScalarType;
147  enum { paramDim = FuncT::NumParams };
148  bool retVal = true;
149  unsigned int totalSize = end - begin;
150  if (!totalSize)
151  {
152  return false;
153  }
154  ScalarType lambda = ScalarType(0.0001);
155  ScalarType* F0 = new ScalarType[totalSize * paramDim];
156  ScalarType* U = new ScalarType[paramDim * paramDim];
157  ScalarType* H = new ScalarType[paramDim * paramDim];
158  ScalarType* v = new ScalarType[paramDim];
159  ScalarType* d = new ScalarType[totalSize];
160  ScalarType* temp = new ScalarType[totalSize];
161  ScalarType* x = new ScalarType[paramDim];
162  ScalarType* p = new ScalarType[paramDim];
163  ScalarType* paramNew = new ScalarType[paramDim];
164  size_t nu = 2;
165  func.Normalize(param);
166  ScalarType paramNorm = 0;
167
168  // do fitting in different steps
169  unsigned int subsets = std::max(int(std::floor(std::log((float)totalSize) / std::log(2.f))) - 8, 1);
170 #ifdef PRECISIONLEVMAR
171  subsets = 1;
172 #endif
173  MiscLib::Vector< unsigned int > subsetSizes(subsets);
174  for (unsigned int i = subsetSizes.size(); i;)
175  {
176  --i;
177  subsetSizes[i] = totalSize;
178  if (i)
179  {
180  subsetSizes[i] = subsetSizes[i] >> 1;
181  }
182  totalSize -= subsetSizes[i];
183  }
184  unsigned int curSubset = 0;
185  unsigned int size = 0;
186
187  // get current error
188  ScalarType chi = 0, newChi = 0;
189  ScalarType rho = 1;
190  unsigned int outerIter = 0,
191 #ifndef PRECISIONLEVMAR
192  maxOuterIter = 200 / subsetSizes.size(),
193 #else
194  maxOuterIter = 500,
195 #endif
196  usefulIter = 0, totalIter = 0;;
197  do
198  {
199  // get current error
200  size += subsetSizes[curSubset];
201  newChi = func.Chi(param, begin, begin + size, d, temp);
202  for (unsigned int i = 0; i < paramDim; ++i)
203  {
204  paramNew[i] = param[i];
205  }
206  outerIter = 0;
207  if (rho < 0)
208  {
209  rho = 1;
210  }
211  do
212  {
213  ++outerIter;
214  ++totalIter;
215  if (rho > 0)
216  {
217  nu = 2;
218  chi = newChi;
219  for (size_t i = 0; i < paramDim; ++i)
220  {
221  param[i] = paramNew[i];
222  }
223 #ifndef PRECISIONLEVMAR
224  if (std::sqrt(chi / size) < ScalarType(1e-5)) // chi very small? -> will be hard to improve
225  {
226  //std::cout << "LevMar converged because of small chi" << std::endl;
227  break;
228  }
229 #endif
230  paramNorm = 0;
231  for (size_t i = 0; i < paramDim; ++i)
232  {
233  paramNorm += param[i] * param[i];
234  }
235  paramNorm = std::sqrt(paramNorm);
236  // construct the needed matrices
237  // F0 is the matrix constructed from param
238  // F0 has gradient_i(param) as its ith row
239  func.Derivatives(param, begin, begin + size, d, temp, F0);
240  // U = F0_t * F0
241  // v = F0_t * d(param) (d(param) = [d_i(param)])
242  #pragma omp parallel for
243  for (int i = 0; i < paramDim; ++i)
244  {
245  for (size_t j = i; j < paramDim; ++j) // j = i since only upper triangle is needed
246  {
247  U[i * paramDim + j] = 0;
248  for (size_t k = 0; k < size; ++k)
249  {
250  U[i * paramDim + j] += F0[k * paramDim + i] *
251  F0[k * paramDim + j];
252  }
253  }
254  }
255  ScalarType vmag = 0; // magnitude of v
256  #pragma omp parallel for
257  for (int i = 0; i < paramDim; ++i)
258  {
259  v[i] = 0;
260  for (size_t k = 0; k < size; ++k)
261  {
262  v[i] += F0[k * paramDim + i] * d[k];
263  }
264  v[i] *= -1;
265 #ifndef DOPARALLEL
266  vmag = std::max(abs(v[i]), vmag);
267 #endif
268  }
269 #ifdef DOPARALLEL
270  for (unsigned int i = 0; i < paramDim; ++i)
271  {
272  vmag = std::max(abs(v[i]), vmag);
273  }
274 #endif
275  // and check for convergence with magnitude of v
276 #ifndef PRECISIONLEVMAR
277  if (vmag < ScalarType(1e-6))
278 #else
279  if (vmag < ScalarType(1e-8))
280 #endif
281  {
282  //std::cout << "LevMar converged with small gradient" << std::endl;
283  //retVal = chi < initialChi;
284  //goto cleanup;
285  break;
286  }
287  if (outerIter == 1)
288  {
289  // compute magnitue of F0
290  ScalarType fmag = abs(F0[0]);
291  for (size_t i = 1; i < paramDim * size; ++i)
292  if (fmag < abs(F0[i]))
293  {
294  fmag = abs(F0[i]);
295  }
296  lambda = 1e-3f * fmag;
297  }
298  else
299  {
300  lambda *= std::max(ScalarType(.3), ScalarType(1 - std::pow(2 * rho - 1, 3)));
301  }
302  }
303
304  memcpy(H, U, sizeof(ScalarType) * paramDim * paramDim);
305  for (size_t i = 0; i < paramDim; ++i)
306  {
307  H[i * paramDim + i] += lambda; // * (ScalarType(1) + H[i * paramDim + i]);
308  }
309  // now H is positive definite and symmetric
310  // solve Hx = -v with Cholesky
311  ScalarType xNorm = 0, L = 0;
312  if (!Cholesky< ScalarType, paramDim >(H, p))
313  {
314  goto increment;
315  }
316  CholeskySolve< ScalarType, paramDim >(H, p, v, x);
317
318  // magnitude of x small? If yes we are done
319  for (size_t i = 0; i < paramDim; ++i)
320  {
321  xNorm += x[i] * x[i];
322  }
323  xNorm = std::sqrt(xNorm);
324 #ifndef PRECISIONLEVMAR
325  if (xNorm <= ScalarType(1e-6) * (paramNorm + ScalarType(1e-6)))
326 #else
327  if (xNorm <= ScalarType(1e-8) * (paramNorm + ScalarType(1e-8)))
328 #endif
329  {
330  //std::cout << "LevMar converged with small step" << std::endl;
331  //goto cleanup;
332  break;
333  }
334
335  for (size_t i = 0; i < paramDim; ++i)
336  {
337  paramNew[i] = param[i] + x[i];
338  }
339  func.Normalize(paramNew);
340
341  // get new error
342  newChi = func.Chi(paramNew, begin, begin + size, d, temp);
343
344  // the following test is taken from
345  // "Methods for non-linear least squares problems"
346  // by Madsen, Nielsen, Tingleff
347  L = 0;
348  for (size_t i = 0; i < paramDim; ++i)
349  {
350  L += .5f * x[i] * (lambda * x[i] + v[i]);
351  }
352  rho = (chi - newChi) / L;
353  if (rho > 0)
354  {
355  ++usefulIter;
356 #ifndef PRECISIONLEVMAR
357  if ((chi - newChi) < 1e-4 * chi)
358  {
359  //std::cout << "LevMar converged with small chi difference" << std::endl;
360  chi = newChi;
361  for (size_t i = 0; i < paramDim; ++i)
362  {
363  param[i] = paramNew[i];
364  }
365  break;
366  }
367 #endif
368  continue;
369  }
370
371 increment:
372  rho = -1;
373  // increment lambda
374  lambda = nu * lambda;
375  size_t nu2 = nu << 1;
376  if (nu2 < nu)
377  {
378  nu2 = 2;
379  }
380  nu = nu2;
381  }
382  while (outerIter < maxOuterIter);
383  ++curSubset;
384  }
385  while (curSubset < subsetSizes.size());
386  retVal = usefulIter > 0;
387  delete[] F0;
388  delete[] U;
389  delete[] H ;
390  delete[] v;
391  delete[] d;
392  delete[] temp;
393  delete[] x;
394  delete[] p;
395  delete[] paramNew;
396  return retVal;
397 }
398
399 template< class ScalarT >
400 ScalarT LevMar(unsigned int paramDim, unsigned int imgDim,
401  const LevMarFunc< ScalarT >** funcs, ScalarT* param)
402 {
403  ScalarT retVal = -1;
404  size_t size = imgDim;
405  float lambda = ScalarT(0.0001);
406  ScalarT* F0 = new ScalarT[size * paramDim];
407  ScalarT* U = new ScalarT[paramDim * paramDim];
408  ScalarT* H = new ScalarT[paramDim * paramDim];
409  ScalarT* v = new ScalarT[paramDim];
410  ScalarT* d = new ScalarT[size];
411  ScalarT* dNew = new ScalarT[size];
412  ScalarT* x = new ScalarT[paramDim];
413  ScalarT* p = new ScalarT[paramDim];
414  ScalarT* paramNew = new ScalarT[paramDim];
415  size_t outerIter = 0, maxOuterIter = 10;
416  // get current error
417  ScalarT chi = 0, newChi;
418  for (size_t i = 0; i < size; ++i)
419  {
420  d[i] = (*(funcs[i]))(param);
421  chi += d[i] * d[i];
422  }
423  do
424  {
425  ++outerIter;
426  lambda *= ScalarT(0.04);
427  // construct the needed matrices
428  // F0 is the matrix constructed from param
429  // F0 has gradient_i(param) as its ith row
430  for (size_t i = 0; i < size; ++i)
431  {
432  (*(funcs[i]))(param, &F0[i * paramDim]);
433  }
434  // U = F0_t * F0
435  for (size_t i = 0; i < paramDim; ++i)
436  for (size_t j = i; j < paramDim; ++j) // j = i since only upper triangle is needed
437  {
438  U[i * paramDim + j] = 0;
439  for (size_t k = 0; k < size; ++k)
440  U[i * paramDim + j] += F0[k * paramDim + i] *
441  F0[k * paramDim + j];
442  }
443  // v = F_t * d(param) (d(param) = [d_i(param)])
444  for (size_t i = 0; i < paramDim; ++i)
445  {
446  v[i] = 0;
447  for (size_t k = 0; k < size; ++k)
448  {
449  v[i] += F0[k * paramDim + i] * d[k];
450  }
451  v[i] *= -1;
452  }
453  size_t iter = 0, maxIter = 10;
454  do
455  {
456  ++iter;
457  // increment lambda
458  lambda = 10 * lambda;
459  memcpy(H, U, sizeof(ScalarT) * paramDim * paramDim);
460  for (size_t i = 0; i < paramDim; ++i)
461  {
462  H[i * paramDim + i] += lambda * (ScalarT(1) + H[i * paramDim + i]);
463  }
464  // now H is positive definite and symmetric
465  // solve Hx = -v with Cholesky
466  if (!Cholesky(H, paramDim, p))
467  {
468  goto cleanup;
469  }
470  CholeskySolve(H, paramDim, p, v, x);
471  for (size_t i = 0; i < paramDim; ++i)
472  {
473  paramNew[i] = param[i] + x[i];
474  }
475  // get new error
476  newChi = 0;
477  for (size_t i = 0; i < size; ++i)
478  {
479  dNew[i] = (*(funcs[i]))(paramNew);
480  newChi += dNew[i] * dNew[i];
481  }
482  // check for convergence
483  /*float cvgTest = 0;
484  for(size_t i = 0; i < paramDim; ++i)
485  {
486  //float c = param[i] - paramNew[i];
487  cvgTest += x[i] * x[i];
488  }
489  if(std::sqrt(cvgTest) < 1e-6)
490  {
491  for(size_t i = 0; i < paramDim; ++i)
492  param[i] = paramNew[i];
493  goto cleanup;
494  }*/
495  if (/*newChi <= chi &&*/ abs(chi - newChi)
496  / chi < ScalarT(1e-4))
497  {
498  for (size_t i = 0; i < paramDim; ++i)
499  {
500  param[i] = paramNew[i];
501  }
502  retVal = newChi;
503  goto cleanup;
504  }
505  }
506  while (newChi > chi && iter < maxIter);
507  if (newChi < chi)
508  {
509  chi = newChi;
510  for (size_t i = 0; i < paramDim; ++i)
511  {
512  param[i] = paramNew[i];
513  }
514  std::swap(d, dNew);
515  }
516  }
517  while (outerIter < maxOuterIter);
518 cleanup:
519  delete[] F0;
520  delete[] U;
521  delete[] H ;
522  delete[] v;
523  delete[] d;
524  delete[] dNew;
525  delete[] x;
526  delete[] p;
527  delete[] paramNew;
528  return retVal;
529 }
530
531 #endif
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
LevMarFunc
Definition: LevMarFunc.h:5
CholeskySolve
void CholeskySolve(ScalarT *a, size_t n, ScalarT p[], ScalarT b[], ScalarT x[])
Definition: LevMarFitting.h:81
Cholesky
bool Cholesky(ScalarT *a, size_t n, ScalarT p[])
Definition: LevMarFitting.h:11
MiscLib::Vector
Definition: Vector.h:19
LevMarFunc.h
armarx::armem::client::util::swap
void swap(SubscriptionHandle &first, SubscriptionHandle &second)
Definition: SubscriptionHandle.cpp:66
MiscLib::Vector::size
size_type size() const
Definition: Vector.h:212
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
LevMar
bool LevMar(IteratorT begin, IteratorT end, FuncT &func, typename FuncT::ScalarType *param)
Definition: LevMarFitting.h:143