00001
00003 #include <iostream>
00004 #include <string>
00005 #include <vector>
00006 #include <fstream>
00007
00008 #include <cstdio>
00009 #include <cstdlib>
00010 #include <cmath>
00011 #include <cassert>
00012
00013
00014 #include "divergence.h"
00015 using namespace std;
00016
00017 Divergence::Divergence (int nb_dimensions)
00018 {
00019
00020 nb_dim=nb_dimensions;
00021 MeanA=new float[nb_dim];
00022 MeanB=new float[nb_dim];
00023 CovA=new float*[nb_dim];
00024 CovB=new float*[nb_dim];
00025 InvCovA=new float*[nb_dim];
00026 InvCovB=new float*[nb_dim];
00027 a=new float[nb_dim];
00028 b=new float[nb_dim];
00029 for (int j=0;j<nb_dim;j++)
00030 {
00031 CovA[j]=new float[nb_dim];
00032 CovB[j]=new float[nb_dim];
00033 InvCovA[j]=new float[nb_dim];
00034 InvCovB[j]=new float[nb_dim];
00035 }
00036 }
00037 Divergence::~Divergence()
00038 {
00039 delete [] MeanA; delete [] MeanB;
00040 for (int j=0;j<nb_dim;j++)
00041 {
00042 delete [] CovA[j]; delete [] CovB[j];
00043 delete [] InvCovA[j]; delete [] InvCovB[j];
00044 }
00045 delete [] CovA; delete [] InvCovA;
00046 delete [] CovB; delete [] InvCovB;
00047 }
00048 void Divergence::Initialise ( float** C_A, float** C_B, float *M_A, float* M_B )
00049 {
00050 int j1,j2;
00051 nb_Gdim=nb_dim;
00052 float aux;
00053 float il=IntLimit;
00054 for (j1=0;j1<nb_dim;j1++)
00055 {
00056 MeanA[j1]=M_A[j1];
00057 MeanB[j1]=M_B[j1];
00058 for (j2=0;j2<nb_dim;j2++)
00059 {
00060 CovA[j1][j2]=C_A[j1][j2];
00061 CovB[j1][j2]=C_B[j1][j2];
00062 }
00063
00064 }
00065
00066
00067
00068
00069 for (j1=0;j1<nb_dim;j1++){
00070 if (CovA[j1][j1]<il||CovB[j1][j1]<il){
00071 nb_Gdim--;
00072 }
00073 }
00074 bool again=true;
00075 while(again){
00076 for (j1=0;j1<nb_dim;j1++)
00077 {
00078 if (CovA[j1][j1]<il||CovB[j1][j1]<il){
00079 if (j1<nb_dim-1){
00080 for (j2=0;j2<nb_dim;j2++){
00081 aux=CovA[j1][j2];
00082 CovA[j1][j2]=CovA[j1+1][j2];
00083 CovA[j1+1][j2]=aux;
00084 aux=CovB[j1][j2];
00085 CovB[j1][j2]=CovB[j1+1][j2];
00086 CovB[j1+1][j2]=aux;
00087
00088 }
00089 for (j2=0;j2<nb_dim;j2++){
00090 aux=CovA[j2][j1];
00091 CovA[j2][j1]=CovA[j2][j1+1];
00092 CovA[j2][j1+1]=aux;
00093 aux=CovB[j2][j1];
00094 CovB[j2][j1]=CovB[j2][j1+1];
00095 CovB[j2][j1+1]=aux;
00096 }
00097 }
00098 }
00099 }
00100 for (j1=0;j1<nb_Gdim;j1++){
00101 if (CovA[j1][j1]<il||CovB[j1][j1]<il){again=true;}
00102 else {again=false;}
00103 }
00104 if (nb_Gdim==0){again=false;}
00105 }
00106
00107 }
00108 void Divergence::InvertCovMatrix()
00109 {
00110 int j1,j2,j,l;
00111 float **auxCov=new float*[nb_Gdim+1];
00112 for (j=0;j<nb_Gdim+1;j++)
00113 {
00114 auxCov[j]=new float[nb_Gdim+1];
00115 }
00116 float *col=new float[nb_Gdim+1];
00117 float d;
00118 int *indx=new int[nb_Gdim+1];
00119 int n=nb_Gdim;
00120 void ludcmp(float **a,int n,int *indx,float *d);
00121 void lubksb(float **a,int n,int *indx, float b[]);
00122 for (j1=0;j1<nb_Gdim;j1++)
00123 {
00124 for (j2=0;j2<nb_Gdim;j2++){auxCov[j1+1][j2+1]=CovA[j1][j2];}
00125
00126 }
00127 ludcmp(auxCov,n,indx,&d);
00128 for (j=1;j<nb_Gdim+1;j++){
00129 for (l=1;l<nb_Gdim+1;l++){col[l]=0.0;}
00130 col[j]=1.0;
00131 lubksb(auxCov,n,indx,col);
00132 for (l=1;l<nb_Gdim+1;l++)
00133 {
00134 InvCovA[l-1][j-1]=col[l];
00135 }
00136 }
00137 for (j=1;j<nb_Gdim+1;j++){
00138 for (l=1;l<nb_Gdim+1;l++){auxCov[j][l]=InvCovA[j-1][l-1];}}
00139 ludcmp(auxCov,n,indx,&d);
00140 for (j=1;j<nb_Gdim+1;j++){d=auxCov[j][j]*d;}
00141 determinantA=d;
00142 for (j1=0;j1<nb_Gdim;j1++)
00143 {
00144 for (j2=0;j2<nb_Gdim;j2++){auxCov[j1+1][j2+1]=CovB[j1][j2];}
00145
00146 }
00147 ludcmp(auxCov,n,indx,&d);
00148 for (j=1;j<nb_Gdim+1;j++){
00149 for (l=1;l<nb_Gdim+1;l++){col[l]=0.0;}
00150 col[j]=1.0;
00151 lubksb(auxCov,n,indx,col);
00152 for (l=1;l<nb_Gdim+1;l++)
00153 {
00154 InvCovB[l-1][j-1]=col[l];
00155 }
00156 }
00157 for (j=1;j<nb_Gdim+1;j++){
00158 for (l=1;l<nb_Gdim+1;l++){auxCov[j][l]=InvCovB[j-1][l-1];}}
00159
00160 ludcmp(auxCov,n,indx,&d);
00161 for (j=1;j<nb_Gdim+1;j++){d=auxCov[j][j]*d;}
00162 determinantB=d;
00163
00164 for (j=0;j<nb_Gdim+1;j++){delete [] auxCov[j];}
00165 delete [] auxCov;delete [] col;delete [] indx;
00166
00167 }
00168
00169 float Divergence::Div_MultiDimFunc(float x,float y, float z)
00170 {
00171 int j , j1 , j2;
00172 float res=0,res1=0,res2=0,res3=1;
00173 float *xx=new float[nb_dim];
00174 xx[0]=x;xx[1]=y;xx[2]=z;
00175 float *auxligne = new float[nb_Gdim];
00176
00177 for (j=0;j<nb_Gdim;j++){auxligne[j]=0;}
00178 for (j1=0;j1<nb_Gdim;j1++)
00179 {
00180 for (j2=0;j2<nb_Gdim;j2++)
00181 {
00182 auxligne[j1]=(xx[j2]-MeanB[j2])*(InvCovB[j1][j2])+auxligne[j1];
00183 }
00184 }
00185 for (j=0;j<nb_Gdim;j++){res1=auxligne[j]*(xx[j]-MeanB[j])+res1;}
00186
00187 for (j=0;j<nb_Gdim;j++){auxligne[j]=0;}
00188 for (j1=0;j1<nb_Gdim;j1++)
00189 {
00190 for (j2=0;j2<nb_Gdim;j2++)
00191 {
00192 auxligne[j1]=(xx[j2]-MeanA[j2])*(InvCovA[j1][j2])+auxligne[j1];
00193 }
00194 }
00195 for (j=0;j<nb_Gdim;j++){res2=auxligne[j]*(xx[j]-MeanA[j])+res2;}
00196
00197 res3=sqrt(determinantA)/(pow(2.0*M_PI,(double)(nb_Gdim/2.)))*exp(-0.5*res2);
00198
00199 res=(0.5*(log(determinantA/determinantB)+(res1-res2)))*res3;
00200
00201 delete [] xx;
00202 delete [] auxligne;
00203 return res;
00204 }
00205
00206 float Divergence::Qgaus(float (Divergence::*func)(float),float a, float b)
00207 {
00208 int j;
00209 float xr,xm,dx,s;
00210 static float x[]={0.0,0.1488743389,0.4333953941,
00211 0.6794095682,0.8650633666,0.9739065285};
00212 static float w[]={0.0,0.2955242247,0.2692667193,
00213 0.2190863625,0.1494513491,0.0666713443};
00214
00215 xm=0.5*(b+a);
00216 xr=0.5*(b-a);
00217 s=0;
00218 for (j=1;j<=5;j++) {
00219 dx=xr*x[j];
00220 s += w[j]*((*this.*func)(xm+dx)+(*this.*func)(xm-dx));
00221 }
00222 return s *= xr;
00223
00224
00225 }
00226
00227 float Divergence::Integration()
00228 {
00229 float res=0;
00230 res=Qgaus(&Divergence::f1,a[0],b[0]);
00231 return res;
00232 }
00233
00234
00235 float Divergence::f1(float x)
00236 {
00237 xsav=x;
00238 return Qgaus(&Divergence::f2,a[1],b[1]);
00239 }
00240 float Divergence::f2(float y)
00241 {
00242 ysav=y;
00243 return Qgaus(&Divergence::f3,a[2],b[2]);
00244 }
00245 float Divergence::f3(float z)
00246 {
00247 return Div_MultiDimFunc(xsav,ysav,z);
00248 }
00249
00250 float Divergence::MeanDifference()
00251 {
00252 float res=0;
00253 for (int i=0;i<nb_dim;i++){res+=(MeanA[i]-MeanB[i])*(MeanA[i]-MeanB[i]);}
00254 return sqrt(res);
00255 }
00256
00257 int Divergence::BoundResolution_Calculation(float *min_bound,float *max_bound,float *resolution,float PrecisionFactor)
00258 {
00259
00260 int indexMax;
00261 int indexMin;
00262 int res=0;
00263 float il=IntLimit;
00264
00265 for (int j=0;j<nb_dim;j++)
00266 {
00267 if (MeanB[j]+sqrt(CovB[j][j])*3.>MeanA[j]+sqrt(CovA[j][j])*3.){
00268 max_bound[j]=MeanB[j]+sqrt(CovB[j][j])*3.;
00269 indexMax=2;
00270 }
00271 else{
00272 max_bound[j]=MeanA[j]+sqrt(CovA[j][j])*3.;
00273 indexMax=1;
00274 }
00275 if (MeanB[j]-sqrt(CovB[j][j])*3.<MeanA[j]-sqrt(CovA[j][j])*3.){
00276 min_bound[j]=MeanB[j]-sqrt(CovB[j][j])*3.;
00277 indexMin=2;
00278 }
00279 else{
00280 min_bound[j]=MeanA[j]-sqrt(CovA[j][j])*3.;
00281 indexMin=1;
00282 }
00283 if (indexMax!=indexMin){
00284 if (indexMax==2){
00285 if ((MeanB[j]-sqrt(CovB[j][j])*3.)>(MeanA[j]+sqrt(CovA[j][j])*3.)){res++;}
00286 }
00287 if (indexMax==1){
00288 if ((MeanA[j]-sqrt(CovA[j][j])*3.)>(MeanB[j]+sqrt(CovB[j][j])*3.)){res++;}
00289 }
00290 }
00291 resolution[j]=-1;
00292 if (CovB[j][j]<il||CovA[j][j]<il){
00293 min_bound[j]=0;
00294 max_bound[j]=1;
00295 resolution[j]=0;
00296 }
00297 }
00298 for (int j=0;j<nb_dim;j++)
00299 {
00300 if (resolution[j]!=0){
00301 if(CovA[j][j]<CovB[j][j])
00302 resolution[j]=sqrt(CovA[j][j])*PrecisionFactor;
00303
00304 else
00305 resolution[j]=sqrt(CovB[j][j])*PrecisionFactor;
00306
00307 if (resolution[j]<=sqrt(il)*PrecisionFactor)
00308 resolution[j]=0;
00309
00310 if ((float)(max_bound[j]-min_bound[j])/resolution[j]>3.){resolution[j]=(float)(max_bound[j]-min_bound[j])/3.;}
00311 if ((float)(max_bound[j]-min_bound[j])/resolution[j]<1.){resolution[j]=(float)(max_bound[j]-min_bound[j])/1.;}
00312
00313 }
00314 }
00315
00316 return res;
00317
00318 }
00319
00320
00321
00322 void Divergence::BoundCalculus(float *pas, float *resolution)
00323 {
00324 int j;
00325 for (j=0;j<nb_dim;j++)
00326 {
00327 a[j]=pas[j];
00328 b[j]=pas[j]+resolution[j];
00329 if (resolution[j]==0){b[j]=a[j]+1;}
00330 }
00331
00332 }
00333
00334
00335 float Divergence::Divergence2GaussModel(float *min_bound,float *max_bound,float *resolution, float weight)
00336 {
00337
00338 double res=0;
00339 float *pas=new float[nb_dim];
00340 float *resolutionaux=new float[nb_dim];
00341 float il=IntLimit;
00342 int maxIteration=0;
00343 resolutionaux[0]=resolution[0];
00344 resolutionaux[1]=resolution[1];
00345 resolutionaux[2]=resolution[2];
00346 float j1=min_bound[0],j2=min_bound[1],j3=min_bound[2];
00347 while (j1<max_bound[0])
00348 {
00349 if (j1+resolutionaux[0]>max_bound[0]){resolutionaux[0]=max_bound[0]-j1;}
00350 pas[0]=j1;
00351 while (j2<max_bound[1])
00352 {
00353 pas[1]=j2;
00354 if (j2+resolutionaux[1]>max_bound[1]){resolutionaux[1]=max_bound[1]-j2;}
00355 while (j3<max_bound[2])
00356 {
00357 pas[2]=j3;
00358 if (j3+resolutionaux[2]>max_bound[2]){resolutionaux[2]=max_bound[2]-j3;}
00359 BoundCalculus(pas,resolutionaux);
00360 res=res+Integration();
00361 maxIteration++;
00362 j3=j3+resolution[2];
00363 if (resolutionaux[2]==0){break;}
00364
00365 }
00366 resolutionaux[2]=resolution[2];
00367 j2=j2+resolution[1];
00368 j3=min_bound[2];
00369 if (resolutionaux[1]==0){break;}
00370
00371 }
00372 resolutionaux[1]=resolution[1];
00373 j1=j1+resolution[0];
00374 j2=min_bound[1];
00375 if (resolutionaux[0]==0){break;}
00376
00377 }
00378
00379 if ((nb_dim-nb_Gdim)>0){
00380 for (int j=0;j<nb_dim;j++){
00381
00382 if (resolution[j]==0){
00383
00384 if (CovA[j][j]<il&&CovB[j][j]>=il){
00385 res+=-log(1./(sqrt(CovB[j][j]*2.*M_PI))*exp(-(MeanA[j]-MeanB[j])*(MeanA[j]-MeanB[j])/(2.*CovB[j][j])));
00386 }
00387 if (CovB[j][j]<il&&CovA[j][j]>=il){
00388 res+=-log(1./(sqrt(CovA[j][j]*2.*M_PI))*exp(-(MeanB[j]-MeanA[j])*(MeanB[j]-MeanA[j])/(2.*CovA[j][j])));
00389 }
00390 if (CovB[j][j]<il&&CovA[j][j]<il){
00391 if (sqrt((MeanB[j]-MeanA[j])*(MeanB[j]-MeanA[j]))>4.*sqrt(il)){res=INF;}
00392
00393 }
00394
00395 }
00396 }
00397 }
00398 res=(float)((double)res/(double)weight);
00399
00400
00401 delete [] pas;
00402 delete [] resolutionaux;
00403 return res;
00404 }
00405