Visual Servoing Platform
version 3.6.1 under development (2024-04-23)
vpLevenbergMarquartd.h
1
/*
2
* ViSP, open source Visual Servoing Platform software.
3
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4
*
5
* This software is free software; you can redistribute it and/or modify
6
* it under the terms of the GNU General Public License as published by
7
* the Free Software Foundation; either version 2 of the License, or
8
* (at your option) any later version.
9
* See the file LICENSE.txt at the root directory of this source
10
* distribution for additional information about the GNU GPL.
11
*
12
* For using ViSP with software that can not be combined with the GNU
13
* GPL, please contact Inria about acquiring a ViSP Professional
14
* Edition License.
15
*
16
* See https://visp.inria.fr for more information.
17
*
18
* This software was developed at:
19
* Inria Rennes - Bretagne Atlantique
20
* Campus Universitaire de Beaulieu
21
* 35042 Rennes Cedex
22
* France
23
*
24
* If you have questions regarding the use of this file, please contact
25
* Inria at visp@inria.fr
26
*
27
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29
*
30
* Description:
31
* Levenberg Marquartd.
32
*/
33
34
#ifndef vpLevenbergMarquartd_h
35
#define vpLevenbergMarquartd_h
36
37
#include <visp3/core/vpConfig.h>
38
#include <visp3/core/vpMath.h>
39
40
#include <errno.h>
41
#include <float.h>
42
#include <math.h>
43
#include <stdio.h>
44
#include <stdlib.h>
45
101
int
VISP_EXPORT qrsolv(
int
n,
double
*r,
int
ldr,
int
*ipvt,
double
*diag,
double
*qtb,
double
*x,
double
*sdiag,
102
double
*wa);
103
104
/*
105
* PROCEDURE : enorm
106
*
107
* ENTREE :
108
*
109
* x Vecteur de taille "n"
110
* n Taille du vecteur "x"
111
*
112
* DESCRIPTION :
113
* La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n"
114
* La norme euclidienne est calculee par accumulation de la somme des carres
115
* dans les trois directions. Les sommes des carres pour les petits et grands
116
* elements sont mis a echelle afin d'eviter les overflows. Des underflows non
117
* destructifs sont autorisee. Les underflows et overflows sont evites dans le
118
* calcul des sommes des carres non encore mis a echelle par les elements
119
* intermediaires. La definition des elements petit, intermediaire et grand
120
* depend de deux constantes : rdwarf et rdiant. Les restrictions principales
121
* sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est
122
* pas en overflow. Les constantes donnees ici conviennent pour la plupart des
123
* pc connus.
124
*
125
* RETOUR :
126
* En cas de succes, la valeur retournee est la norme euclidienne du vecteur
127
* Sinon, la valeur -1 est retournee et la variable globale "errno" est
128
* initialisee pour indiquee le type de l'erreur.
129
*/
130
double
VISP_EXPORT enorm(
const
double
*x,
int
n);
131
132
/* PROCEDURE : lmpar
133
*
134
* ENTREE :
135
* n Ordre de la matrice "r".
136
* r Matrice de taille "n" x "n". En entree, la toute la partie
137
* triangulaire superieure doit contenir toute la partie
138
* triangulaire superieure de "r".
139
*
140
* ldr Taille maximum de la matrice "r". "ldr" >= "n".
141
*
142
* ipvt Vecteur de taille "n" qui definit la matrice de permutation
143
* "p" tel que : a * p = q * r. La jeme colonne de p la colonne ipvt[j] de la
144
* matrice d'identite.
145
*
146
* diag Vecteur de taille "n" contenant les elements diagonaux de la
147
* matrice "d".
148
*
149
* qtb Vecteur de taille "n" contenant les "n" premiers elements du
150
* vecteur (q transpose)*b.
151
*
152
* delta Limite superieure de la norme euclidienne de d * x.
153
*
154
* par Estimee initiale du parametre de Levenberg-Marquardt.
155
* wa1, wa2 Vecteurs de taille "n" de travail.
156
*
157
* DESCRIPTION :
158
* La procedure determine le parametre de Levenberg-Marquardt. Soit une
159
* matrice "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de
160
* taille "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta,
161
* le probleme est le calcul du parametre "par" de telle facon que si "x"
162
* resoud le systeme
163
*
164
* a * x = b , sqrt(par) * d * x = 0 ,
165
*
166
* au sens des moindre carre, et dxnorm est la norme euclidienne de d * x
167
* alors "par" vaut 0.0 et (dxnorm - delta) <= 0.1 * delta ,
168
* ou "par" est positif et abs(dxnorm-delta) <= 0.1 * delta.
169
* Cette procedure complete la solution du probleme si on lui fourni les infos
170
* nessaires de la factorisation qr, avec pivotage de colonnes de a.
171
* Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
172
* de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
173
* avec les elements diagonaux classes par ordre decroissant de leur valeur,
174
* lmpar attend une matrice triangulaire superieure complete, la matrice de
175
* permutation "p" et les "n" premiers elements de (q transpose) * b. En
176
* sortie, la procedure lmpar fournit aussi une matrice triangulaire
177
* superieure "s" telle que
178
*
179
* t t t
180
* p * (a * a + par * d * d )* p = s * s .
181
*
182
* "s" est utilise a l'interieure de lmpar et peut etre d'un interet separe.
183
*
184
* Seulement quelques iterations sont necessaire pour la convergence de
185
* l'algorithme. Si neanmoins la limite de 10 iterations est atteinte, la
186
* valeur de sortie "par" aura la derniere meilleure valeur.
187
*
188
* SORTIE :
189
* r En sortie, tout le triangle superieur est inchange, et le
190
* le triangle inferieur contient les elements de la partie
191
* triangulaire superieure (transpose) de la matrice triangulaire
192
* superieure de "s".
193
* par Estimee finale du parametre de Levenberg-Marquardt.
194
* x Vecteur de taille "n" contenant la solution au sens des
195
* moindres carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le
196
* parametre en sortie "par"
197
* sdiag Vecteur de taille "n" contenant les elements diagonaux de la
198
* matrice triangulaire "s".
199
*
200
* RETOUR :
201
* En cas de succes, la valeur 0.0 est retournee.
202
*/
203
int
VISP_EXPORT lmpar(
int
n,
double
*r,
int
ldr,
int
*ipvt,
double
*diag,
double
*qtb,
double
*delta,
double
*par,
204
double
*x,
double
*sdiag,
double
*wa1,
double
*wa2);
205
206
/*
207
* PROCEDURE : pythag
208
*
209
* ENTREES :
210
* a, b Variables dont on veut la racine carre de leur somme de carre
211
*
212
* DESCRIPTION :
213
* La procedure calcule la racine carre de la somme des carres de deux nombres
214
* en evitant l'overflow ou l'underflow destructif.
215
*
216
* RETOUR :
217
* La procedure retourne la racine carre de a^2 + b^2.
218
*/
219
double
VISP_EXPORT pythag(
double
a,
double
b);
220
221
/*
222
* PROCEDURE : qrfac
223
*
224
* ENTREE :
225
* m Nombre de lignes de la matrice "a".
226
* n Nombre de colonne de la matrice "a".
227
* a Matrice de taille "m" x "n". elle contient, en entree la
228
*matrice dont on veut sa factorisation qr.
229
* lda Taille maximale de "a". lda >= m.
230
* pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est
231
*realise Si pivot = FALSE, pas de pivotage.
232
* lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de
233
* l'ordre de 1. Sinon lipvt est de l'ordre de "n".
234
* wa Vecteur de travail de taille "n". Si pivot = FALSE "wa"
235
* coincide avec rdiag.
236
*
237
* DESCRIPTION :
238
* La procedure effectue une decomposition de la matrice "a"par la methode qr.
239
* Elle utilise les transformations de householders avec pivotage sur les
240
* colonnes (option) pour calculer la factorisation qr de la matrice "a" de
241
* taille "m" x "n". La procedure determine une matrice orthogonale "q", une
242
* matrice de permutation "p" et une matrice trapesoidale superieure "r" dont
243
* les elements diagonaux sont ordonnes dans l'ordre decroissant de leurs
244
* valeurs,tel que a * p = q * r. La transformation de householder pour la
245
* colonne k, k = 1,2,...,min(m,n), est de la forme
246
* t
247
* i - (1 / u(k)) * u * u
248
*
249
* Ou u a des zeros dans les k-1 premieres positions.
250
*
251
* SORTIE :
252
* a Matrice de taille "m" x "n" dont le trapeze superieur de "a"
253
* contient la partie trapezoidale superieure de "r" et la partie
254
* trapezoidale inferieure de "a" contient une forme factorisee
255
* de "q" (les elements non triviaux du vecteurs "u" sont decrits
256
* ci-dessus).
257
* ipvt Vecteur de taille "n". Il definit la matrice de permutation
258
* "p" tel que a * p = q * r. La jeme colonne de p est la colonne ipvt[j] de la
259
* matrice d'identite. Si pivot = FALSE, ipvt n'est pas referencee. rdiag
260
* Vecteur de taille "n" contenant les elements diagonaux de la matrice
261
* "r". acnorm Vecteur de taille "n" contenant les normes des lignes
262
* correspondantes de la matrice "a". Si cette information n'est
263
* pas requise, acnorm coincide avec rdiag.
264
*/
265
int
VISP_EXPORT qrfac(
int
m,
int
n,
double
*a,
int
lda,
int
*pivot,
int
*ipvt,
int
lipvt,
double
*rdiag,
double
*acnorm,
266
double
*wa);
267
268
/*
269
* PROCEDURE : lmder
270
*
271
*
272
* ENTREE :
273
* fcn Fonction qui calcule la fonction et le jacobien de la
274
* fonction. m Nombre de fonctions.
275
* n Nombre de variables. n <= m
276
* x Vecteur de taille "n" contenant en entree une estimation
277
* initiale de la solution.
278
* ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m".
279
* ftol Erreur relative desiree dans la somme des carre. La
280
* terminaison survient quand les preductions estimee et vraie de la somme des
281
* carres sont toutes deux au moins egal a ftol.
282
* xtol Erreur relative desiree dans la solution approximee. La
283
* terminaison survient quand l'erreur relative entre deux
284
* iterations consecutives est au moins egal a xtol.
285
* gtol Mesure de l'orthogonalite entre le vecteur des fonctions et
286
* les colonnes du jacobien. La terminaison survient quand le cosinus de
287
* l'angle entre fvec et n'importe quelle colonne du jacobien est au moins
288
* egal a gtol, en valeur absolue. maxfev Nombre d'appel maximum. La
289
* terminaison se produit lorsque le nombre d'appel a fcn avec iflag = 1 a
290
* atteint "maxfev".
291
* diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est
292
* initialisee en interne. Si mode = 2, diag doit contenir les
293
* entree positives qui servent de facteurs d'echelle aux
294
* variables.
295
* mode Si mode = 1, les variables seront mis a l'echelle en interne.
296
* Si mode = 2, la mise a l'echelle est specifie par l'entree
297
* diag. Les autres valeurs de mode sont equivalents a mode = 1. factor
298
* Definit la limite de l'etape initial. Cette limite est initialise au
299
* produit de "factor" et de la norme euclidienne de "diag" * "x" sinon nul.
300
* ou a "factor" lui meme. Dans la plupart des cas, "factor" doit se trouve
301
* dans l'intervalle (1, 100); ou 100 est la valeur recommandee. nprint
302
* Controle de l'impression des iterees (si valeur positive). Dans ce
303
* cas, fcn est appelle avec iflag = 0 au debut de la premiere iteration et
304
* apres chaque nprint iteration, x, fvec, et fjac sont disponible pour
305
* impression, cela avant de quitter la procedure. Si "nprint" est negatif,
306
* aucun appel special de fcn est faite. wa1, wa2, wa3 Vecteur de travail de
307
* taille "n". wa4 Vecteur de travail de taille "m".
308
*
309
* SORTIE :
310
* x Vecteur de taille "n" contenant en sortie l'estimee finale
311
* de la solution.
312
* fvec Vecteur de taille "m" contenant les fonctions evaluee en "x".
313
* fjac Matrice de taille "m" x "n". La sous matrice superieure de
314
* taille "n" x "n" de fjac contient une matrice triangulaire
315
* superieure r dont les elements diagonaux, classe dans le sens
316
* decroissant de leur valeur, sont de la forme :
317
*
318
* T T T
319
* p * (jac * jac) * p = r * r
320
*
321
* Ou p est une matrice de permutation et jac est le jacobien
322
* final calcule.
323
* La colonne j de p est la colonne ipvt (j) (voir ci apres) de
324
* la matrice identite. La partie trapesoidale inferieure de fjac
325
* contient les information genere durant le calcul de r.
326
* info Information de l'execution de la procedure. Lorsque la
327
* procedure a termine son execution, "info" est inialisee a la valeur
328
* (negative) de iflag. sinon elle prend les valeurs suivantes :
329
* info = 0 : parametres en entree non valides.
330
* info = 1 : les reductions relatives reelle et estimee de la
331
* somme des carres sont au moins egales a ftol.
332
* info = 2 : erreur relative entre deux iteres consecutives sont
333
* egaux a xtol.
334
* info = 3 : conditions info = 1 et info = 2 tous deux requis.
335
* info = 4 : le cosinus de l'angle entre fvec et n'importe
336
* quelle colonne du jacobien est au moins egal a gtol, en valeur absolue. info
337
* = 5 : nombre d'appels a fcn avec iflag = 1 a atteint maxfev. info = 6 :
338
* ftol est trop petit. Plus moyen de reduire de la somme des carres. info =
339
* 7 : xtol est trop petit. Plus d'amelioration possible pour approximer la
340
* solution x. info = 8 : gtol est trop petit. "fvec" est orthogonal aux
341
* colonnes du jacobien a la precision machine pres.
342
* nfev Nombre d'appel a "fcn" avec iflag = 1.
343
* njev Nombre d'appel a "fcn" avec iflag = 2.
344
* ipvt Vecteur de taille "n". Il definit une matrice de permutation p
345
* tel que jac * p = q * p, ou jac est le jacbien final calcule,
346
* q est orthogonal (non socke) et r est triangulaire superieur,
347
* avec les elements diagonaux classes en ordre decroissant de
348
* leur valeur. La colonne j de p est ipvt[j] de la matrice
349
* identite. qtf Vecteur de taille n contenant les n premiers elements
350
* du vecteur qT * fvec.
351
*
352
* DESCRIPTION :
353
* La procedure minimize la somme de carre de m equation non lineaire a n
354
* variables par une modification de l'algorithme de Levenberg - Marquardt.
355
*
356
* REMARQUE :
357
* L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et
358
* le jacobien.
359
* "fcn" doit etre declare dans une instruction externe a la procedure et doit
360
* etre appele comme suit :
361
* fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
362
* *iflag)
363
*
364
* si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
365
* fjac n'est pas modifie.
366
* si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac.
367
* fvec n'est pas modifie.
368
*
369
* RETOUR :
370
* En cas de succes, la valeur zero est retournee.
371
* Sinon la valeur -1 est retournee.
372
*/
373
int
VISP_EXPORT lmder(
void
(*ptr_fcn)(
int
m,
int
n,
double
*xc,
double
*fvecc,
double
*jac,
int
ldfjac,
int
iflag),
374
int
m,
int
n,
double
*x,
double
*fvec,
double
*fjac,
int
ldfjac,
double
ftol,
double
xtol,
375
double
gtol,
unsigned
int
maxfev,
double
*diag,
int
mode,
const
double
factor,
int
nprint,
376
int
*info,
unsigned
int
*nfev,
int
*njev,
int
*ipvt,
double
*qtf,
double
*wa1,
double
*wa2,
377
double
*wa3,
double
*wa4);
378
379
/*
380
* PROCEDURE : lmder1
381
*
382
* ENTREE :
383
* fcn Fonction qui calcule la fonction et le jacobien de la
384
* fonction. m Nombre de fonctions. n Nombre de variables
385
* (parametres). n <= m x Vecteur de taille "n" contenant en
386
* entree une estimation initiale de la solution.
387
* ldfjac Taille maximale de la matrice "fjac". ldfjac >= "m".
388
* tol Tolerance. La terminaison de la procedure survient quand
389
* l'algorithme estime que l'erreur relative dans la somme des
390
* carres est au moins egal a tol ou bien que l'erreur relative
391
* entre x et la solution est au moins egal atol.
392
* wa Vecteur de travail de taille "lwa".
393
* lwa Taille du vecteur "wa". wa >= 5 * n + m.
394
*
395
*
396
* SORTIE :
397
* x Vecteur de taille "n" contenant en sortie l'estimee finale
398
* de la solution.
399
* fvec Vecteur de taille "m" contenant les fonctions evaluee en "x".
400
* fjac Matrice de taille "m" x "n". La sous matrice superieure de
401
* taille "n" x "n" de fjac contient une matrice triangulaire
402
* superieure r dont les elements diagonaux, classe dans le sens
403
* decroissant de leur valeur, sont de la forme :
404
*
405
* T T T
406
* p * (jac * jac) * p = r * r
407
*
408
* Ou p est une matrice de permutation et jac est le jacobien
409
* final calcule.
410
* La colonne j de p est la colonne ipvt (j) (voir ci apres) de
411
* la matrice identite. La partie trapesoidale inferieure de fjac
412
* contient les information genere durant le calcul de r.
413
* info Information de l'executionde la procedure. Lorsque la
414
* procedure a termine son execution, "info" est inialisee a la valeur
415
* (negative) de iflag. sinon elle prend les valeurs suivantes :
416
* info = 0 : parametres en entre non valides.
417
* info = 1 : estimation par l'algorithme que l'erreur relative
418
* de la somme des carre est egal a tol.
419
* info = 2 : estimation par l'algorithme que l'erreur relative
420
* entre x et la solution est egal a tol.
421
* info = 3 : conditions info = 1 et info = 2 tous deux requis.
422
* info = 4 : fvec est orthogonal aux colonnes du jacobien.
423
* info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint
424
* 100 * (n + 1).
425
* info = 6 : tol est trop petit. Plus moyen de reduire de la
426
* somme des carres.
427
* info = 7 : tol est trop petit. Plus d'amelioration possible
428
* d' approximer la solution x.
429
* ipvt Vecteur de taille "n". Il definit une matrice de permutation p
430
* tel que jac * p = q * p, ou jac est le jacbien final calcule,
431
* q est orthogonal (non socke) et r est triangulaire superieur,
432
* avec les elements diagonaux classes en ordre decroissant de
433
* leur valeur. La colonne j de p est ipvt[j] de la matrice
434
* identite.
435
*
436
* DESCRIPTION :
437
* La procedure minimize la somme de carre de m equation non lineaire a n
438
* variables par une modification de l'algorithme de Levenberg - Marquardt.
439
* Cette procedure appele la procedure generale au moindre carre lmder.
440
*
441
* REMARQUE :
442
* L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et
443
* le jacobien.
444
* "fcn" doit etre declare dans une instruction externe a la procedure et doit
445
* etre appele comme suit :
446
* fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
447
* *iflag)
448
*
449
* si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
450
* fjac n'est pas modifie.
451
* si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac.
452
* fvec n'est pas modifie.
453
*
454
* RETOUR :
455
* En cas de succes, la valeur zero est retournee.
456
* Sinon, la valeur -1.
457
*/
458
int
VISP_EXPORT lmder1(
void
(*ptr_fcn)(
int
m,
int
n,
double
*xc,
double
*fvecc,
double
*jac,
int
ldfjac,
int
iflag),
459
int
m,
int
n,
double
*x,
double
*fvec,
double
*fjac,
int
ldfjac,
double
tol,
int
*info,
460
int
*ipvt,
int
lwa,
double
*wa);
461
462
#endif
visp
modules
vision
src
pose-estimation
private
vpLevenbergMarquartd.h
Generated by
1.9.1