/*
* PROCEDURE : lmder
*
*
* ENTREE :
* fcn Fonction qui calcule la fonction et le jacobien de la fonction.
* m Nombre de fonctions.
* n Nombre de variables. n <= m
* x Vecteur de taille "n" contenant en entree une estimation
* initiale de la solution.
* ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m".
* ftol Erreur relative desiree dans la somme des carre. La terminaison
* survient quand les preductions estimee et vraie de la somme des
* carres sont toutes deux au moins egal a ftol.
* xtol Erreur relative desiree dans la solution approximee. La
* terminaison survient quand l'erreur relative entre deux
* iterations consecutives est au moins egal a xtol.
* gtol Mesure de l'orthogonalité entre le vecteur des fonctions et les
* colonnes du jacobien. La terminaison survient quand le cosinus
* de l'angle entre fvec et n'importe quelle colonne du jacobien
* est au moins egal a gtol, en valeur absolue.
* maxfev Nombre d'appel maximum. La terminaison se produit lorsque le
* nombre d'appel a fcn avec iflag = 1 a atteint "maxfev".
* diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est
* initialisee en interne. Si mode = 2, diag doit contenir les
* entree positives qui servent de facteurs d'echelle aux variables.
* mode Si mode = 1, les variables seront mis a l'echelle en interne.
* Si mode = 2, la mise a l'echelle est specifie par l'entree diag.
* Les autres valeurs de mode sont equivalents a mode = 1.
* factor Definit la limite de l'etape initial. Cette limite est initialise
* au produit de "factor" et de la norme euclidienne de "diag" * "x"
* sinon nul. ou a "factor" lui meme. Dans la plupart des cas,
* "factor" doit se trouve dans l'intervalle (1, 100); ou 100 est
* la valeur recommandee.
* nprint Controle de l'impression des iterees (si valeur positive).
* Dans ce cas, fcn est appelle avec iflag = 0 au debut de la
* premiere iteration et apres chaque nprint iteration, x, fvec,
* et fjac sont disponible pour impression, cela avant de quitter
* la procedure. Si "nprint" est negatif, aucun appel special de
* fcn est faite.
* wa1, wa2, wa3 Vecteur de travail de taille "n".
* wa4 Vecteur de travail de taille "m".
*
*
* SORTIE :
* x Vecteur de taille "n" contenant en sortie l'estimee finale
* de la solution.
* fvec Vecteur de taille "m" contenant les fonctions evaluee en "x".
* fjac Matrice de taille "m" x "n". La sous matrice superieure de
* taille "n" x "n" de fjac contient une matrice triangulaire
* superieure r dont les elements diagonaux, classe dans le sens
* decroissant de leur valeur, sont de la forme :
*
* T T T
* p * (jac * jac) * p = r * r
*
* Ou p est une matrice de permutation et jac est le jacobien
* final calcule.
* La colonne j de p est la colonne ipvt (j) (voir ci apres) de
* la matrice identite. La partie trapesoidale inferieure de fjac
* contient les informations genere durant le calcul de r.
* info Information de l'execution de la procedure. Lorsque la procedure
* a termine son execution, "info" est inialisee a la valeur
* (negative) de iflag. sinon elle prend les valeurs suivantes :
* info = 0 : parametres en entree non valides.
* info = 1 : les reductions relatives reelle et estimee de la
* somme des carres sont au moins egales a ftol.
* info = 2 : erreur relative entre deux iteres consecutives sont
* egaux a xtol.
* info = 3 : conditions info = 1 et info = 2 tous deux requis.
* info = 4 : le cosinus de l'angle entre fvec et n'importe quelle
* colonne du jacobien est au moins egal a gtol, en
* valeur absolue.
* info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint
* maxfev.
* info = 6 : ftol est trop petit. Plus moyen de reduire de la
* somme des carres.
* info = 7 : xtol est trop petit. Plus d'amelioration possible
* pour approximer la solution x.
* info = 8 : gtol est trop petit. "fvec" est orthogonal aux
* colonnes du jacobien a la precision machine pres.
* nfev Nombre d'appel a "fcn" avec iflag = 1.
* njev Nombre d'appel a "fcn" avec iflag = 2.
* ipvt Vecteur de taille "n". Il definit une matrice de permutation p
* tel que jac * p = q * p, ou jac est le jacbien final calcule,
* q est orthogonal (non socke) et r est triangulaire superieur,
* avec les elements diagonaux classes en ordre decroissant de
* leur valeur. La colonne j de p est ipvt[j] de la matrice identite.
* qtf Vecteur de taille n contenant les n premiers elements du
* vecteur qT * fvec.
*
* DESCRIPTION :
* La procedure minimize la somme de carre de m equation non lineaire a n
* variables par une modification de l'algorithme de Levenberg - Marquardt.
*
* REMARQUE :
* L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et
* le jacobien.
* "fcn" doit etre declare dans une instruction externe a la procedure et doit
* etre appele comme suit :
* fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
*
* si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
* fjac n'est pas modifie.
* si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac.
* fvec n'est pas modifie.
*
* RETOUR :
* En cas de succes, la valeur zero est retournee.
* Sinon la valeur -1 est retournee.
*/
int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
double *jac, int ldfjac, int iflag), int m, int n, double *x,
double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
double gtol, int maxfev, double *diag, int mode,
const double factor, int nprint, int *info, int *nfev,
int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
double *wa3, double *wa4)
{
const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
int oncol = TRUE;
int iflag, iter, count = 0;
int i, j, l;
double actred, delta, dirder, epsmch, fnorm, fnorm1, gnorm;
double ratio = DBL_EPSILON;
double par, pnorm, prered;
double sum, temp, temp1, temp2, xnorm = 0.0;
/* epsmch est la precision machine. */
epsmch = DBL_EPSILON;
*info = 0;
iflag = 0;
*nfev = 0;
*njev = 0;
/* verification des parametres d'entree. */
if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0)
|| (gtol < 0.0) || (maxfev <= 0) || (factor <= 0.0))
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
if (mode == 2)
{
for (j = 0; j < n; j++)
{
if (diag[j] <= 0.0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
}
}
/*
* evaluation de la fonction au point de depart
* et calcul de sa norme.
*/
iflag = 1;
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
*nfev = 1;
if (iflag < 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
fnorm = enorm(fvec, m);
/*
* initialisation du parametre de Levenberg-Marquardt
* et du conteur d'iteration.
*/
par = 0.0;
iter = 1;
/*
* debut de la boucle la plus externe.
*/
while (count < maxfev)
{
count++;
/*
* calcul de la matrice jacobienne.
*/
iflag = 2;
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
(*njev) ++;
if (iflag < 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
/*
* si demandee, appel de fcn pour impression des iterees.
*/
if (nprint > 0)
{
iflag = 0;
if ((iter-1) % nprint == 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
if (iflag < 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
}
/*
* calcul de la factorisation qr du jacobien.
*/
qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3);
/*
* a la premiere iteration et si mode est 1, mise a l'echelle
* en accord avec les normes des colonnes du jacobien initial.
*/
if (iter == 1)
{
if (mode != 2)
{
for (j = 0; j < n; j++)
{
diag[j] = wa2[j];
if (wa2[j] == 0.0)
diag[j] = 1.0;
}
}
/*
* a la premiere iteration, calcul de la norme de x mis
* a l'echelle et initialisation de la limite delta de
* l'etape.
*/
for (j = 0; j < n; j++)
wa3[j] = diag[j] * x[j];
xnorm = enorm (wa3, n);
delta = factor * xnorm;
if (delta == 0.0)
delta = factor;
}
/*
* formation de (q transpose) * fvec et stockage des n premiers
* composants dans qtf.
*/
for (i = 0; i < m; i++)
wa4[i] = fvec[i];
for (i = 0; i < n; i++)
{
if (*MIJ(fjac, i, i, ldfjac) != 0.0)
{
sum = 0.0;
for (j = i; j < m; j++)
sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
temp = - sum / *MIJ(fjac, i, i, ldfjac);
for (j = i; j < m; j++)
wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
}
*MIJ(fjac, i, i, ldfjac) = wa1[i];
qtf[i] = wa4[i];
}
/*
* calcul de la norme du gradient mis a l'echelle.
*/
gnorm = 0.0;
if (fnorm != 0.0)
{
for (i = 0; i < n; i++)
{
l = ipvt[i];
if (wa2[l] != 0.0)
{
sum = 0.0;
for (j = 0; j <= i; j++)
sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
gnorm = vpMath::maximum(gnorm, fabs(sum / wa2[l]));
}
}
}
/*
* test pour la convergence de la norme du gradient .
*/
if (gnorm <= gtol)
*info = 4;
if (*info != 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
/*
* remise a l'echelle si necessaire.
*/
if (mode != 2)
{
for (j = 0; j < n; j++)
diag[j] = vpMath::maximum(diag[j], wa2[j]);
}
/*
* debut de la boucle la plus interne.
*/
ratio = 0.0;
while (ratio < tol0001)
{
/*
* determination du parametre de Levenberg-Marquardt.
*/
lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1,
wa2, wa3, wa4);
/*
* stockage de la direction p et x + p. calcul de la norme de p.
*/
for (j = 0; j < n; j++)
{
wa1[j] = - wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
pnorm = enorm(wa3, n);
/*
* a la premiere iteration, ajustement de la premiere limite de
* l'etape.
*/
if (iter == 1)
delta = vpMath::minimum(delta, pnorm);
/*
* evaluation de la fonction en x + p et calcul de leur norme.
*/
iflag = 1;
(*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag);
(*nfev)++;
if (iflag < 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
fnorm1 = enorm(wa4, m);
/*
* calcul de la reduction reelle mise a l'echelle.
*/
actred = - 1.0;
if ((tol1 * fnorm1) < fnorm)
actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
/*
* calcul de la reduction predite mise a l'echelle et
* de la derivee directionnelle mise a l'echelle.
*/
for (i = 0; i < n; i++)
{
wa3[i] = 0.0;
l = ipvt[i];
temp = wa1[l];
for (j = 0; j <= i; j++)
wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp;
}
temp1 = enorm(wa3, n) / fnorm;
temp2 = (sqrt(par) * pnorm) / fnorm;
prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
dirder = - ((temp1 * temp1) + (temp2 * temp2));
/*
* calcul du rapport entre la reduction reel et predit.
*/
ratio = 0.0;
if (prered != 0.0)
ratio = actred / prered;
/*
* mise a jour de la limite de l'etape.
*/
if (ratio > tol25)
{
if ((par == 0.0) || (ratio <= tol75))
{
delta = pnorm / tol5;
par *= tol5;
}
}
else
{
if (actred >= 0.0)
temp = tol5;
else
temp = tol5 * dirder / (dirder + tol5 * actred);
if ((tol1 * fnorm1 >= fnorm) || (temp < tol1))
temp = tol1;
delta = temp * vpMath::minimum(delta, (pnorm / tol1));
par /= temp;
}
/*
* test pour une iteration reussie.
*/
if (ratio >= tol0001)
{
/*
* iteration reussie. mise a jour de x, de fvec, et de
* leurs normes.
*/
for (j = 0; j < n; j++)
{
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
for (i = 0; i < m; i++)
fvec[i] = wa4[i];
xnorm = enorm(wa2, n);
fnorm = fnorm1;
iter++;
}
/*
* tests pour convergence.
*/
if ((fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0))
*info = 1;
if (delta <= xtol * xnorm)
*info = 2;
if ((fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)
&& *info == 2)
*info = 3;
if (*info != 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m,n,x,fvec,fjac,ldfjac, iflag);
return (count);
}
/*
* tests pour termination et
* verification des tolerances.
*/
if (*nfev >= maxfev)
*info = 5;
if ((fabs(actred) <= epsmch) && (prered <= epsmch)
&& (tol5 * ratio <= 1.0))
*info = 6;
if (delta <= epsmch * xnorm)
*info = 7;
if (gnorm <= epsmch)
*info = 8;
if (*info != 0)
{
/*
* termination, normal ou imposee par l'utilisateur.
*/
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
}/* fin while ratio >=tol0001 */
}/*fin while 1*/
return 0 ;
}
/*
* PROCEDURE : firy_n_float
*
* INPUT :
* ima_in Pointeur sur la pyramide de l'image.
*
* OUTPUT :
* ima_filt Pointeur sur la pyramide de gradient spatial.
* On obtient les gradients suivant x ou y en fonction de
* la valeur des coefficients du filtre contenus dans "coefs".
*
* INPUTS :
* coefs Pointeur sur les coefficients du filtre.
* taille Taille du filtre.
*
* DESCRIPTION :
* La procedure effectue un filtrage taille X taille de
* l'image "ima_in". Ce filtrage correspond au calcul d'un gradients,
* suivant x ou y, selon la valeur des coefficients du filtre contenus
* dans "coefs"
*
* HISTORIQUE :
* 1.00 - 01/01/95 - Original.
*/
static bool firy_n_float (TImageShort *ima_in, TImageFloat *ima_filt,
float *coefs, size_t taille)
{
size_t midsize = taille / 2;
size_t xsize = (size_t) ima_in->nbco;
size_t ysize = (size_t) ima_in->nbli;
void (*fir_float) (const float *, float *, size_t, const float *);
float *src;
float *dst;
size_t y;
// Test si filtre applicable
if ((xsize < midsize) || (ysize < midsize)) {
return false;
}
switch (taille) {
case 3 : fir_float = firf3sym_float; break;
case 5 : fir_float = firf5sym_float; break;
case 7 : fir_float = firf7sym_float; break;
default : return false;
}
if ((src = (float *) malloc (xsize * sizeof(float))) == (float *) NULL)
return false;
if ((dst = (float *) malloc (xsize * sizeof(float))) == (float *) NULL) {
free ((void *) src);
return false;
}
set_float (ima_filt->ad, 0.F, xsize * ysize);
for (y = 0; y < ysize; y++) {
int i;
cast_short_float (MIJ(ima_in->ad, y, 0, xsize), src, xsize);
for (i = 0; i < (int) midsize; i++) {
(*fir_float) (src, dst, xsize - (taille - 1),
MIJ(coefs, i, 0, taille));
if ((y + midsize >= (size_t) i) && (y + midsize < ysize + i))
add_float (
MIJ(ima_filt->ad, y + midsize - i, midsize, xsize),
dst, xsize - (taille - 1));
if (y + i >= midsize && y + i < ysize + midsize)
subtract_float (
MIJ(ima_filt->ad, y + i - midsize, midsize, xsize),
dst, xsize - (taille - 1));
}
}
for (y = 0; y < midsize; y++) {
set_float (MIJ(ima_filt->ad, y, 0, xsize), 0.F, xsize);
set_float (MIJ(ima_filt->ad, ysize - y - 1, 0, xsize), 0.F,
xsize);
}
free ((void *) src);
free ((void *) dst);
return true;
}
/* PROCEDURE : qrsolv
*
* ENTREE :
* n Ordre de la matrice "r".
* r Matrice de taille "n" x "n". En entree, la partie triangulaire
* complete de "r" doit contenir la partie triangulaire superieure
* complete de "r".
* ldr Taille maximale de la matrice "r". "ldr" >= n.
* ipvt Vecteur de taille "n" definissant la matrice de permutation "p"
* La jeme colonne de de "p" est la colonne ipvt[j] de la matrice
* identite.
* diag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice "d".
* qtb Vecteur de taille "n" contenant les "n" premiers elements du
* vecteur (q transpose) * b.
* wa Vecteur de travail de taille "n".
*
* DESCRIPTION :
* La procedure complete la solution du probleme, si on fournit les informations
* necessaires de la factorisation qr, avec pivotage des colonnes.
* Soit une matrice "a" de taille "m" x "n" donnee, une matrice diagonale "d" de
* taille "n" x "n" et un vecteur "b" de taille "m", le probleme est la determination
* un vecteur "x" qui est solution du systeme
*
* a*x = b , d*x = 0 ,
*
* Au sens des moindres carres.
*
* Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de "q"
* sont orthogonales et "r" est une matrice traingulaire superieure dont les
* elements diagonaux sont classes de l'ordre decroissant de leur valeur. Cette
* procedure attend donc la matrice triangulaire superieure remplie "r", la
* matrice de permutaion "p" et les "n" premiers elements de (q transpose) * b.
* Le systeme
*
* a * x = b, d * x = 0, est alors equivalent a
*
* t t
* r * z = q * b , p * d * p * z = 0 ,
*
* Ou x = p * z. Si ce systeme ne possede pas de rangee pleine, alors une
* solution au moindre carre est obtenue. En sortie, la procedure fournit aussi
* une matrice triangulaire superieure "s" tel que
*
* t t t
* p * (a * a + d * d) * p = s * s .
*
* "s" est calculee a l'interieure de qrsolv et peut etre hors interet.
*
* SORTIE :
* r En sortie, le triangle superieur n'est pas altere, et la partie
* triangulaire inferieure contient la partie triangulaire superieure
* (transpose) de la matrice triangulaire "s".
* x Vecteur de taille "n" contenant les solutions au moindres carres du
* systeme a * x = b, d * x = 0.
* sdiag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice triangulaire superieure "s".
*
*/
int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
double *x, double *sdiag, double *wa)
{
int i, j, jp1, k, kp1, l; /* compteur de boucle */
int nsing;
double cosi, cotg, qtbpj, sinu, sum, tg, temp;
/*
* copie de r et (q transpose) * b afin de preserver l'entree
* et initialisation de "s". En particulier, sauvegarde des elements
* diagonaux de "r" dans "x".
*/
for (i = 0; i < n; i++)
{
for (j = i; j < n; j++)
*MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
x[i] = *MIJ(r, i, i, ldr);
wa[i] = qtb[i];
}
/*
* Elimination de la matrice diagonale "d" en utlisant une rotation
* connue.
*/
for (i = 0; i < n; i++)
{
/*
* preparation de la colonne de d a eliminer, reperage de
* l'element diagonal par utilisation de p de la
* factorisation qr.
*/
l = ipvt[i];
if (diag[l] != 0.0)
{
for (k = i; k < n; k++)
sdiag[k] = 0.0;
sdiag[i] = diag[l];
/*
* Les transformations qui eliminent la colonne de d
* modifient seulement qu'un seul element de
* (q transpose)*b avant les n premiers elements
* lesquels sont inialement nuls.
*/
qtbpj = 0.0;
for (k = i; k < n; k++)
{
/*
* determination d'une rotation qui elimine
* les elements appropriees dans la colonne
* courante de d.
*/
if (sdiag[k] != 0.0)
{
if (fabs(*MIJ(r, k, k, ldr)) >= fabs(sdiag[k]))
{
tg = sdiag[k] / *MIJ(r, k, k, ldr);
cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
sinu = cosi * tg;
}
else
{
cotg = *MIJ(r, k, k, ldr) / sdiag[k];
sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
cosi = sinu * cotg;
}
/*
* calcul des elements de la diagonale modifiee
* de r et des elements modifies de
* ((q transpose)*b,0).
*/
*MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k];
temp = cosi * wa[k] + sinu * qtbpj;
qtbpj = -sinu * wa[k] + cosi * qtbpj;
wa[k] = temp;
/*
* accumulation des tranformations dans
* les lignes de s.
*/
kp1 = k + 1;
if ( n >= kp1)
{
for (j = kp1; j < n; j++)
{
temp = cosi * *MIJ(r, k, j, ldr) +
sinu * sdiag[j];
sdiag[j] = - sinu * *MIJ(r, k, j, ldr) +
cosi * sdiag[j];
*MIJ(r, k, j, ldr) = temp;
}
}
}/* fin if diag[] !=0 */
} /* fin boucle for k -> n */
}/* fin if diag =0 */
/*
* stokage de l'element diagonal de s et restauration de
* l'element diagonal correspondant de r.
*/
sdiag[i] = *MIJ(r, i, i, ldr);
*MIJ(r, i, i, ldr) = x[i];
} /* fin boucle for j -> n */
/*
* resolution du systeme triangulaire pour z. Si le systeme est
* singulier, on obtient une solution au moindres carrés.
*/
nsing = n;
for (i = 0; i < n; i++)
{
if (sdiag[i] == 0.0 && nsing == n)
nsing = i - 1;
if (nsing < n)
wa[i] = 0.0;
}
if (nsing >= 0)
{
for (k = 0; k < nsing; k++)
{
i = nsing - 1 - k;
sum = 0.0;
jp1 = i + 1;
if (nsing >= jp1)
{
for (j = jp1; j < nsing; j++)
sum += *MIJ(r, i, j, ldr) * wa[j];
}
wa[i] = (wa[i] - sum) / sdiag[i];
}
}
/*
* permutation arriere des composants de z et des componants de x.
*/
for (j = 0; j < n; j++)
{
l = ipvt[j];
x[l] = wa[j];
}
return (0);
}
/*
* PROCEDURE : qrfac
*
* ENTREE :
* m Nombre de lignes de la matrice "a".
* n Nombre de colonne de la matrice "a".
* a Matrice de taille "m" x "n". elle contient, en entree la matrice
* dont on veut sa factorisation qr.
* lda Taille maximale de "a". lda >= m.
* pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est realise
* Si pivot = FALSE, pas de pivotage.
* lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de
* l'ordre de 1. Sinon lipvt est de l'ordre de "n".
* wa Vecteur de travail de taille "n". Si pivot = FALSE "wa"
* coincide avec rdiag.
*
* DESCRIPTION :
* La procedure effectue une decomposition de la matrice "a"par la methode qr.
* Elle utilise les transformations de householders avec pivotage sur les colonnes
* (option) pour calculer la factorisation qr de la matrice "a" de taille "m" x "n".
* La procedure determine une matrice orthogonale "q", une matrice de permutation
* "p" et une matrice trapesoidale superieure "r" dont les elements diagonaux
* sont ordonnes dans l'ordre decroissant de leurs valeurs,tel que a * p = q * r.
* La transformation de householder pour la colonne k, k = 1,2,...,min(m,n),
* est de la forme
* t
* i - (1 / u(k)) * u * u
*
* Ou u a des zeros dans les k-1 premieres positions.
*
* SORTIE :
* a Matrice de taille "m" x "n" dont le trapeze superieur de "a"
* contient la partie trapezoidale superieure de "r" et la partie
* trapezoidale inferieure de "a" contient une forme factorisee
* de "q" (les elements non triviaux du vecteurs "u" sont decrits
* ci-dessus).
* ipvt Vecteur de taille "n". Il definit la matrice de permutation "p"
* tel que a * p = q * r. La jeme colonne de p est la colonne
* ipvt[j] de la matrice d'identite. Si pivot = FALSE, ipvt n'est
* pas referencee.
* rdiag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice "r".
* acnorm Vecteur de taille "n" contenant les normes des lignes
* correspondantes de la matrice "a". Si cette information n'est
* pas requise, acnorm coincide avec rdiag.
*
*/
int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
int /* lipvt */, double *rdiag, double *acnorm, double *wa)
{
const double tolerance = 0.05;
int i, j, ip1, k, kmax, minmn;
double ajnorm, epsmch;
double sum, temp, tmp;
/*
* epsmch est la precision machine.
*/
epsmch = DBL_EPSILON;
/*
* calcul des normes initiales des lignes et initialisation
* de plusieurs tableaux.
*/
for (i = 0; i < m; i++)
{
acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
rdiag[i] = acnorm[i];
wa[i] = rdiag[i];
if (pivot)
ipvt[i] = i;
}
/*
* reduction de "a" en "r" avec les tranformations de Householder.
*/
minmn = vpMath::minimum(m, n);
for (i = 0; i < minmn; i++)
{
if (pivot)
{
/*
* met la ligne de plus grande norme en position
* de pivot.
*/
kmax = i;
for (k = i; k < m; k++)
{
if (rdiag[k] > rdiag[kmax])
kmax = k;
}
if (kmax != i)
{
for (j = 0; j < n; j++)
SWAP(*MIJ(a, i, j, lda),
*MIJ(a, kmax, j, lda), tmp);
rdiag[kmax] = rdiag[i];
wa[kmax] = wa[i];
SWAP( ipvt[i], ipvt[kmax], k);
}
}
/*
* calcul de al transformationde Householder afin de reduire
* la jeme ligne de "a" a un multiple du jeme vecteur unite.
*/
ajnorm = enorm(MIJ(a, i, i, lda), n - i);
if (ajnorm != 0.0)
{
if (*MIJ(a, i, i, lda) < 0.0)
ajnorm = -ajnorm;
for (j = i; j < n; j++)
*MIJ(a, i, j, lda) /= ajnorm;
*MIJ(a, i, i, lda) += 1.0;
/*
* application de la tranformation aux lignes
* restantes et mise a jour des normes.
*/
ip1 = i + 1;
if (m >= ip1)
{
for (k = ip1; k < m; k++)
{
sum = 0.0;
for (j = i; j < n; j++)
sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
temp = sum / *MIJ(a, i, i, lda);
for (j = i; j < n; j++)
*MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
if (pivot && rdiag[k] != 0.0)
{
temp = *MIJ (a, k, i, lda) / rdiag[k];
rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch)
{
rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n -1 - (int) i));
wa[k] = rdiag[k];
}
}
}/* fin boucle for k */
}
} /* fin if (ajnorm) */
rdiag[i] = -ajnorm;
} /* fin for (i = 0; i < minmn; i++) */
return (0);
}
/* PROCEDURE : lmpar
*
* ENTREE :
* n Ordre de la matrice "r".
* r Matrice de taille "n" x "n". En entree, la toute la partie
* triangulaire superieure doit contenir toute la partie triangulaire
* superieure de "r".
*
* ldr Taille maximum de la matrice "r". "ldr" >= "n".
*
* ipvt Vecteur de taille "n" qui definit la matrice de permutation "p"
* tel que :
* a * p = q * r.
* La jeme colonne de p la colonne ipvt[j] de la matrice d'identite.
*
* diag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice "d".
*
* qtb Vecteur de taille "n" contenant les "n" premiers elements du
* vecteur (q transpose)*b.
*
* delta Limite superieure de la norme euclidienne de d * x.
*
* par Estimee initiale du parametre de Levenberg-Marquardt.
* wa1, wa2 Vecteurs de taille "n" de travail.
*
* DESCRIPTION :
* La procedure determine le parametre de Levenberg-Marquardt. Soit une matrice
* "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de taille
* "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta, le probleme
* est le calcul du parametre "par" de telle facon que si "x" resoud le systeme
*
* a * x = b , sqrt(par) * d * x = 0 ,
*
* au sens des moindre carre, et dxnorm est la norme euclidienne de d * x
* alors "par" vaut 0.0 et (dxnorm - delta) <= 0.1 * delta ,
* ou "par" est positif et abs(dxnorm-delta) <= 0.1 * delta.
* Cette procedure complete la solution du probleme si on lui fourni les infos
* nessaires de la factorisation qr, avec pivotage de colonnes de a.
* Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
* de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
* avec les elements diagonaux classes par ordre decroissant de leur valeur, lmpar
* attend une matrice triangulaire superieure complete, la matrice de permutation
* "p" et les "n" premiers elements de (q transpose) * b. En sortie, la procedure
* lmpar fournit aussi une matrice triangulaire superieure "s" telle que
*
* t t t
* p * (a * a + par * d * d )* p = s * s .
*
* "s" est utilise a l'interieure de lmpar et peut etre d'un interet separe.
*
* Seulement quelques iterations sont necessaire pour la convergence de
* l'algorithme. Si neanmoins la limite de 10 iterations est atteinte, la
* valeur de sortie "par" aura la derniere meilleure valeur.
*
* SORTIE :
* r En sortie, tout le triangle superieur est inchange, et le
* le triangle inferieur contient les elements de la partie
* triangulaire superieure (transpose) de la matrice triangulaire
* superieure de "s".
* par Estimee finale du parametre de Levenberg-Marquardt.
* x Vecteur de taille "n" contenant la solution au sens des moindres
* carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le
* parametre en sortie "par"
* sdiag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice triangulaire "s".
*
* RETOUR :
* En cas de succes, la valeur 0.0 est retournee.
*
*/
int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
double *delta, double *par, double *x, double *sdiag, double *wa1, double *wa2)
{
const double tol1 = 0.1, tol001 = 0.001; /* tolerance a 0.1 et a 0.001 */
long i, j, jm1, jp1, k, l; /* compteur de boucle */
int iter; /* compteur d'iteration */
int nsing; /* nombre de singularite de la matrice */
double dxnorm, fp, gnorm, parc;
double parl, paru; /* borne inf et sup de par */
double sum, temp;
double dwarf = DBL_MIN; /* plus petite amplitude positive */
/*
* calcul et stockage dans "x" de la direction de Gauss-Newton. Si
* le jacobien a une rangee de moins, on a une solution au moindre
* carres.
*/
nsing = n;
for (i = 0; i < (long) n; i++)
{
wa1[i] = qtb[i];
if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
nsing = (int) i - 1;
if (nsing < n)
wa1[i] = 0.0;
}
if ((int) nsing >= 0)
{
for (k = 0; k < (long) nsing; k++)
{
i = nsing - 1 - k;
wa1[i] /= *MIJ(r, i, i, ldr);
temp = wa1[i];
jm1 = i - 1;
if (jm1 >= 0)
{
for (j = 0; j <= jm1; j++)
wa1[j] -= *MIJ(r, i, j, ldr) * temp;
}
}
}
for (j = 0; j < (long) n; j++)
{
l = ipvt[j];
x[l] = wa1[j];
}
/*
* initialisation du compteur d'iteration.
* evaluation de la fonction a l'origine, et test
* d'acceptation de la direction de Gauss-Newton.
*/
iter = 0;
for (i = 0; i < (long) n; i++)
wa2[i] = diag[i] * x[i];
dxnorm = enorm(wa2, n);
fp = dxnorm - *delta;
if (fp > tol1 * (*delta))
{
/*
* Si le jacobien n'a pas de rangee deficiente,l'etape de
* Newton fournit une limite inferieure, parl pour le
* zero de la fonction. Sinon cette limite vaut 0.0.
*/
parl = 0.0;
if (nsing >= n)
{
for (i = 0; i < (long) n; i++)
{
l = ipvt[i];
wa1[i] = diag[l] * (wa2[l] / dxnorm);
}
for (i = 0; i < (long) n; i++)
{
long im1;
sum = 0.0;
im1 = (i - 1L);
if (im1 >= 0)
{
for (j = 0; j <= im1; j++)
sum += (*MIJ(r, i, j, ldr) * wa1[j]);
}
wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr);
}
temp = enorm(wa1, n);
parl = ((fp / *delta) / temp) / temp;
}
/*
* calcul d'une limite superieure, paru, pour le zero de la
* fonction.
*/
for (i = 0; i < (long) n; i++)
{
sum = 0.0;
for (j = 0; j <= i; j++)
sum += *MIJ(r, i, j, ldr) * qtb[j];
l = ipvt[i];
wa1[i] = sum / diag[l];
}
gnorm = enorm(wa1, n);
paru = gnorm / *delta;
if (paru == 0.0)
paru = dwarf / vpMath::minimum(*delta, tol1);
/*
* Si "par" en entree tombe hors de l'intervalle (parl,paru),
* on le prend proche du point final.
*/
*par = vpMath::maximum(*par, parl);
*par = vpMath::minimum(*par, paru);
if (*par == 0.0)
*par = gnorm / dxnorm;
/*
* debut d'une iteration.
*/
while (iter >= 0)
{
iter++;
/*
* evaluation de la fonction a la valeur courant
* de "par".
*/
if (*par == 0.0)
*par = vpMath::maximum(dwarf, (tol001 * paru));
temp = sqrt(*par);
for (i = 0; i < (long) n; i++)
wa1[i] = temp * diag[i];
qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2);
for (i = 0; i < (long) n; i++)
wa2[i] = diag[i] * x[i];
dxnorm = enorm(wa2, n);
temp = fp;
fp = dxnorm - *delta;
/*
* si la fonction est assez petite, acceptation de
* la valeur courant de "par". de plus, test des cas
* ou parl est nul et ou le nombre d'iteration a
* atteint 10.
*/
if ((fabs(fp) <= tol1 * *delta) || ((parl == 0.0) && (fp <= temp)
&& (temp < 0.0)) || (iter == 10))
{
/*
* terminaison.
*/
if (iter == 0)
*par = 0.0;
return (0);
}
/*
* calcul de la correction de Newton.
*/
for (i = 0; i < (long) n; i++)
{
l = ipvt[i];
wa1[i] = diag[l] * (wa2[l] / dxnorm);
}
for (i = 0; i < (long) n; i++)
{
wa1[i] = wa1[i] / sdiag[i];
temp = wa1[i];
jp1 = i + 1;
if ( (long) n >= jp1)
{
for (j = jp1; j < (long) n; j++)
wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
}
}
temp = enorm(wa1, n);
parc = ((fp / *delta) / temp) / temp;
/*
* selon le signe de la fonction, mise a jour
* de parl ou paru.
*/
if (fp > 0.0)
parl = vpMath::maximum(parl, *par);
if (fp < 0.0)
paru = vpMath::minimum(paru, *par);
/*
* calcul d'une estimee ameliree de "par".
*/
*par = vpMath::maximum(parl, (*par + parc));
}/* fin boucle sur iter */
}/* fin fp > tol1 * delta */
/*
* terminaison.
*/
if (iter == 0)
*par = 0.0;
return (0);
}
/*
INPUTS :
imgx Spatial gradients under x.
imgy Spatial gradients under y.
imgt Temporal gradient (DFD): imgt = I(pi+depl,t+1) - I(pi,t)
zone_val Estimation support.
etiq Support value to take into consderation.
fen Work window.
ima_pond Ponderation.
OUTPUT :
d_param Estimated motion model in the polynomial form.
DESCRIPTION :
Compute a robust estimation of the motion model.
RETURN :
The number of pixels used for the computation.
*/
bool estimate_QUA_COMPLET(TImageFloat *imgx, TImageFloat *imgy,
TImageFloat *imgt, TImageShort *zone_val,
int etiq, TWindow win,
TImageFloat *ima_pond, Para *d_param)
{
int N; // Number of parameters
N = d_param->nb_para;
if (d_param->var_light)
N++;
int cnt = 0; // Pixel counter
int nb_pts_utiles = 0; // Used pixel counter
double YTWY = 0.0;
double YTWXTheta = 0.0;
double *A_ = new double [N*N];
double **A = new double* [N];
double *B = new double [N];
double *X = new double [N];
double *phi = new double [N];
double li_c = d_param->li_c;
double co_c = d_param->co_c;
int success = 0; // return value
int x; // column
int y; // row
int i, j;
for (i=0; i < N; i ++)
A[i] = A_ + i*N;
d_param->sigma2res = 0.0;
set_double (X, 0.0, N);
set_double (B, 0.0, N);
set_double (&A[0][0], 0.0, N * N);
// Diagonal pertubation, to assume that the matrix would be inversible
for (i = 0; i < N; i++)
A[i][i] = PERTURBATION;
if (d_param->var_light)
phi[N-1] = 1.0; // For the global illumination variation parameter
for (y = win.dli; y <win.fli; y++) {
size_t off = MIJ(0, y, 0, ima_pond->nbco);
short *pval = &zone_val->ad[off];
float *pond = &ima_pond->ad[off];
float *pgx = &imgx->ad[off];
float *pgy = &imgy->ad[off];
float *pgt = &imgt->ad[off];
double dy = (double) (y - li_c);
for(x = win.dco; x < win.fco; x++) {
double dpond, dpgt; // temporary values
double dx;
if ((pval[x]==etiq) && (((pgx[x]!=0.0)||(pgy[x]!=0.0))||(pgt[x]!=0.0))){
nb_pts_utiles++;
if (pond[x] > POND_MINI) {
cnt++;
dpond = (double) pond[x];
dpgt = (double) pgt[x];
dx = (double) (x - co_c);
phi[0] = pgx[x];
phi[1] = pgy[x];
phi[2] = phi[0] * dx;
phi[3] = phi[0] * dy;
phi[4] = phi[1] * dx;
phi[5] = phi[1] * dy;
phi[6] = phi[2] * dx;
phi[7] = phi[2] * dy;
phi[8] = phi[3] * dy;
phi[9] = phi[4] * dx;
phi[10] = phi[4] * dy;
phi[11] = phi[5] * dy;
// For the residual variance computation
if (d_param->compute_sigma2res)
YTWY += dpond * dpgt * dpgt;
for (i = 0; i < N; i++) {
double d = (double) phi[i] * dpond;
B[i] -= d * dpgt;
// Cumulation on a triangle matrix
for (j = 0; j <= i; j++)
A[i][j] += phi[j] * d;
}
}
}
}
}
// Fill the upper triangle of the matrix
for (i = 0; i < N; i++)
for (j = i + 1; j < N; j++)
A[i][j] = A[j][i];
d_param->tx_pts = ((double)cnt)/nb_pts_utiles;
if (cnt <= N * 2) { // Not enough pixels
delete [] A_;
delete [] A;
delete [] B;
delete [] X;
delete [] phi;
return false;
}
// Solve AX = B by exploiting the symetry of the matrix
success = resoud_mat_sym (&A[0][0], B, X, N, N);
if (! success) {
set_double (X, 0.0, N);
return false;
}
// Convert the compact form to the polynomial form
if (d_param->var_light) {
for (i=0; i < N-1; i ++)
d_param->thet[i] = X[i];
d_param->thet[12] = X[N-1]; // Illumination variation
}
else {
for (i=0; i < N; i ++)
d_param->thet[i] = X[i];
}
if ( ! d_param->compute_sigma2res) {
delete [] A_;
delete [] A;
delete [] B;
delete [] X;
delete [] phi;
return true;
}
// Compute the residual variance
YTWXTheta = dot_double (B, X, N);
d_param->sigma2res = (YTWY - YTWXTheta) / (double) cnt;
if ( ! d_param->compute_covariance) {
delete [] A_;
delete [] A;
delete [] B;
delete [] X;
delete [] phi;
return true;
}
// Compute the covariance matrix
double *InvA_ = new double [N*N];
double **InvA = new double* [N];
for (i=0; i < N; i ++)
InvA[i] = InvA_ + i*N;
success = inverse_mat_sym (&A[0][0], &InvA[0][0], N);
if (! success) {
return false;
}
update_covariance(&InvA[0][0], d_param);
delete [] A_;
delete [] A;
delete [] B;
delete [] X;
delete [] phi;
delete [] InvA_;
delete [] InvA;
return true;
}
/*
* PROCEDURE : fcn
*
* ENTREES :
* m Nombre d'equations.
* n Nombre de variables.
* xc Valeur courante des parametres.
* fvecc Resultat de l'evaluation de la fonction.
* ldfjac Plus grande dimension de la matrice jac.
* iflag Choix du calcul de la fonction ou du jacobien.
*
* SORTIE :
* jac Jacobien de la fonction.
*
* DESCRIPTION :
* La procedure calcule la fonction et le jacobien.
* Si iflag == 1, la procedure calcule la fonction en "xc" et le resultat est
* stocke dans "fvecc" et "fjac" reste inchange.
* Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
* stocke dans "fjac" et "fvecc" reste inchange.
*
* HISTORIQUE :
* 1.00 - xx/xx/xx - Original.
* 1.01 - 06/07/95 - Modifications.
* 2.00 - 24/10/95 - Tableau jac monodimensionnel.
*/
void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
{
double u[X3_SIZE];// rd[X3_SIZE][X3_SIZE],
vpRotationMatrix rd ;
int npt;
if (m < n) printf("pas assez de points\n");
npt = m / 2;
if (iflag == 1) eval_function (npt, xc, fvecc);
else if (iflag == 2)
{
double u1, u2, u3;
u[0] =xc[3];
u[1]= xc[4];
u[2]= xc[5];
rd.buildFrom(u[0],u[1],u[2]) ;
/* a partir de l'axe de rotation, calcul de la matrice de rotation. */
// rot_mat(u, rd);
double tt = sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */
if (tt >= MINIMUM)
{
u1 = u[0] / tt;
u2 = u[1] / tt; /* axe de rotation unitaire */
u3 = u[2] / tt;
}
else u1 = u2 = u3 = 0.0;
double co = cos(tt);
double mco = 1.0 - co;
double si = sin(tt);
for (int i = 0; i < npt; i++)
{
double x = XO[i];
double y = YO[i]; /* coordonnees du point i */
double z = ZO[i];
/* coordonnees du point i dans le repere camera */
double rx = rd[0][0] * x + rd[0][1] * y + rd[0][2] * z + xc[0];
double ry = rd[1][0] * x + rd[1][1] * y + rd[1][2] * z + xc[1];
double rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
/* derive des fonctions rx, ry et rz par rapport
* a tt, u1, u2, u3.
*/
double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y
+ (si * u1 * u1 - si) * x;
double drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
double drxu2 = si * z + mco * u1 * y;
double drxu3 = mco * u1 * z - si * y;
double dryt = (si * u2 * u3 - co * u1) * z + (si * u2 * u2 - si) * y
+ (co * u3 + si * u1 * u2) * x;
double dryu1 = mco * u2 * x - si * z;
double dryu2 = mco * u3 * z + 2 * mco * u2 * y + mco * u1 * x;
double dryu3 = mco * u2 * z + si * x;
double drzt = (si * u3 * u3 - si) * z + (si * u2 * u3 + co * u1) * y
+ (si * u1 * u3 - co * u2) * x;
double drzu1 = si * y + mco * u3 * x;
double drzu2 = mco * u3 * y - si * x;
double drzu3 = 2 * mco * u3 * z + mco * u2 * y + mco * u1 * x;
/* derive de la fonction representant le modele de la
* camera (sans distortion) par rapport a tt, u1, u2 et u3.
*/
double dxit = drxt / rz - rx * drzt / (rz * rz);
double dyit = dryt / rz - ry * drzt / (rz * rz);
double dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
double dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
double dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
double dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
double dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
double dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
/* calcul du jacobien : le jacobien represente la
* derivee de la fonction representant le modele de la
* camera par rapport aux parametres.
*/
*MIJ(jac, 0, i, ldfjac) = 1 / rz;
*MIJ(jac, 1, i, ldfjac) = 0.0;
*MIJ(jac, 2, i, ldfjac) = - rx / (rz * rz);
if (tt >= MINIMUM)
{
*MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt
- u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
*MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt
+ (1 - u2 * u2) * dxiu2 / tt- u2 * u3 * dxiu3 / tt;
*MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt
+ (1 - u3 * u3) * dxiu3 / tt;
}
else
{
*MIJ(jac, 3, i, ldfjac) = 0.0;
*MIJ(jac, 4, i, ldfjac) = 0.0;
*MIJ(jac, 5, i, ldfjac) = 0.0;
}
*MIJ(jac, 0, npt + i, ldfjac) = 0.0;
*MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
*MIJ(jac, 2, npt + i, ldfjac) = - ry / (rz * rz);
if (tt >= MINIMUM)
{
*MIJ(jac, 3, npt + i, ldfjac) = u1 * dyit + (1 - u1 * u1) * dyiu1 / tt
- u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
*MIJ(jac, 4, npt + i, ldfjac) = u2 * dyit - u1 * u2 * dyiu1 / tt
+ (1 - u2 * u2) * dyiu2 / tt- u2 * u3 * dyiu3 / tt;
*MIJ(jac, 5, npt + i, ldfjac) = u3 * dyit - u1 * u3 * dyiu1 / tt
- u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
}
else
{
*MIJ(jac, 3, npt + i, ldfjac) = 0.0;
*MIJ(jac, 4, npt + i, ldfjac) = 0.0;
*MIJ(jac, 5, npt + i, ldfjac) = 0.0;
}
}
} /* fin else if iflag ==2 */
}
