image_matrix.c

LastWave

// CHANGED 15/07
/*..........................................................................*/
/*                                                                          */
/*      L a s t W a v e    P a c k a g e 'image' 2.1                        */
/*                                                                          */
/*      Copyright (C) 1998-2002 Emmanuel Bacry.                             */
/*      emails : [e-mail]                               */
/*               [e-mail]                             */
/*                                                                          */
/*..........................................................................*/
/*                                                                          */
/*      This program is a free software, you can redistribute it and/or     */
/*      modify it under the terms of the GNU General Public License as      */
/*      published by the Free Software Foundation; either version 2 of the  */
/*      License, or (at your option) any later version                      */
/*                                                                          */
/*      This program is distributed in the hope that it will be useful,     */
/*      but WITHOUT ANY WARRANTY; without even the implied warranty of      */
/*      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the       */
/*      GNU General Public License for more details.                        */
/*                                                                          */
/*      You should have received a copy of the GNU General Public License   */
/*      along with this program (in a file named COPYRIGHT);                */
/*      if not, write to the Free Software Foundation, Inc.,                */
/*      59 Temple Place, Suite 330, Boston, MA  02111-1307  USA             */
/*                                                                          */
/*..........................................................................*/



/****************************************************************************/
/*                                                                          */
/*  image_matrix.c   Functions which deal with matrices                     */
/*                                                                          */
/****************************************************************************/



#include "lastwave.h"
#include "signals.h"
#include "images.h"

/*
 * Useful structures
 */
 
/* Floating matrix structure */
typedef struct fmatrix {
 LWFLOAT **Y;
 int size;
 int sizeMalloc;
} *FMATRIX;

/* Floating Vector structure */
typedef struct fvector {
  LWFLOAT *Y;
  int   size;
} *FVECTOR;


/* Integer Vector structure */
typedef struct ivector {
 int *Y;
 int size;
} *IVECTOR;

/*
 * (Des)Allocation
 */
 
/* Creation (and Allocation) of a New square FMATRIX of size size */
FMATRIX NewFMatrix(int size)
{
  FMATRIX cm;
  int i;

  cm = (FMATRIX) Malloc(sizeof(struct fmatrix));
  cm->size = size;
  cm->sizeMalloc = size;

  cm->Y = (LWFLOAT **) Malloc(sizeof(LWFLOAT *)*size) -1;  
  for(i=1;i<=size;i++) cm->Y[i] = FloatAlloc(size)-1;
  
  return(cm);
}

/* Delete a FMATRIX Structure */
void DeleteFMatrix(FMATRIX cm)
{
  int i;
  
  if (cm) {
    for(i=1;i<=cm->sizeMalloc;i++) Free(cm->Y[i]+1);
	Free(cm->Y+1);
  }

  Free(cm);
}

FMATRIX Image2FMatrix(IMAGE im)
{
  FMATRIX m;
  int i,k;

  if (im->nrow != im->ncol) Errorf("Image2FMatrix() : Not square image");
  
  m = NewFMatrix(im->nrow);
  
  for (k=0,i=1;i<=im->nrow;i++) {
    memcpy(m->Y[i]+1,im->pixels+k,im->ncol*sizeof(LWFLOAT));
    k+=im->ncol;
  }
  
  return(m);
}

void FMatrix2Image(FMATRIX m, IMAGE im)
{
  int i,k;
  
  SizeImage(im,m->size,m->size);
  
  for (k=0,i=1;i<=im->nrow;i++) {
    memcpy(im->pixels+k,m->Y[i]+1,im->ncol*sizeof(LWFLOAT));
    k+=im->ncol;
  }
}

/* Creation (and Allocation) of a New IVECTOR of size size */
IVECTOR NewIvector(int size)
{
  IVECTOR ov;
  
  ov = (IVECTOR) Malloc(sizeof(struct ivector));
   
  ov->size = size;
  ov->Y = IntAlloc(size)-1;

  return(ov);
}

/* Delete a IVECTOR Structure */
void DeleteIVector(IVECTOR ov)
{
  if (ov)
    {
      Free(ov->Y+1);
      Free(ov);
    }
}

/* Creation (and Allocation) of a New FVECTOR of size size */
FVECTOR NewFVector(int size)
{
  FVECTOR ov;
  
  ov = (FVECTOR) Malloc(sizeof(struct fvector));
   
  ov->size = size;
  ov->Y = FloatAlloc(size)-1;

  return(ov);
}

void FVector2Signal(FVECTOR v, SIGNAL s)
{
  SizeSignal(s,v->size,YSIG);
  memcpy(s->Y,v->Y+1,v->size*sizeof(LWFLOAT));
}

/* Delete a FVECTOR Structure */
void DeleteFVector(FVECTOR ov)
{
  if (ov)
    {
      Free(ov->Y+1);
      Free(ov);
    }
}


/*
 * Math routines 
 */

/* Matrix Inversion using LU decomposition */
#define TINY 1.0e-20;
static void ludcmp(FMATRIX a,IVECTOR indx,LWFLOAT *d)
{
  
  int n = a->size;
  int i,imax,j,k;
  LWFLOAT big,dum,sum,temp;
  FVECTOR vv;

  vv=NewFVector(n);
  *d=1.0;
  for (i=1;i<=n;i++) {
    big=0.0;
    for (j=1;j<=n;j++)
      if ((temp=fabs(a->Y[i][j])) > big) big=temp;
    if (big == 0.0) Errorf("ludcmp(): Singular matrix");
    vv->Y[i]=1.0/big;
  }
  for (j=1;j<=n;j++) {
    for (i=1;i<j;i++) {
      sum=a->Y[i][j];
      for (k=1;k<i;k++) sum -= a->Y[i][k]*a->Y[k][j];
      a->Y[i][j]=sum;
    }
    big=0.0;
    for (i=j;i<=n;i++) {
      sum=a->Y[i][j];
      for (k=1;k<j;k++)
	sum -= a->Y[i][k]*a->Y[k][j];
      a->Y[i][j]=sum;
      if ( (dum=vv->Y[i]*fabs(sum)) >= big) {
	big=dum;
	imax=i;
      }
    }
    if (j != imax) {
      for (k=1;k<=n;k++) {
	dum=a->Y[imax][k];
	a->Y[imax][k]=a->Y[j][k];
	a->Y[j][k]=dum;
      }
      *d = -(*d);
      vv->Y[imax]=vv->Y[j];
    }
    indx->Y[j]=imax;
    if (a->Y[j][j] == 0.0) a->Y[j][j]=TINY;
    if (j != n) {
      dum=1.0/(a->Y[j][j]);
      for (i=j+1;i<=n;i++) a->Y[i][j] *= dum;
    }
  }
  DeleteFVector(vv);
}
#undef TINY

static void lubksb(FMATRIX a,IVECTOR indx,FVECTOR b)
{
  int n = a->size;
  int i,ii=0,ip,j;
  LWFLOAT sum;
  
  for (i=1;i<=n;i++) {
    ip=indx->Y[i];
    sum=b->Y[ip];
    b->Y[ip]=b->Y[i];
    if (ii)
      for (j=ii;j<=i-1;j++) sum -= a->Y[i][j]*b->Y[j];
    else if (sum) ii=i;
    b->Y[i]=sum;
  }
  for (i=n;i>=1;i--) {
    sum=b->Y[i];
    for (j=i+1;j<=n;j++) sum -= a->Y[i][j]*b->Y[j];
    b->Y[i]=sum/a->Y[i][i];
  }
}
 
void InverseFMatrix(FMATRIX m) 
{
  int n = m->size;
  LWFLOAT d;
  IVECTOR indx;
  FVECTOR col;
  FMATRIX res;
  int i,j;

  indx = NewIvector(n);
  col = NewFVector(n);
  res = NewFMatrix(n);
  
  ludcmp(m,indx,&d);
  for (j=1;j<=n;j++) {
    for (i=1;i<=n;i++) col->Y[i] = 0.0;
    col->Y[j] = 1.0;
    lubksb(m,indx,col);
    for(i=1;i<=n;i++) res->Y[i][j] = col->Y[i];
  }
  
  for (j=1;j<=n;j++) 
    for (i=1;i<=n;i++) m->Y[i][j] = res->Y[i][j];
  
  DeleteIVector(indx);
  DeleteFVector(col);
  DeleteFMatrix(res);
}

void InverseImage(IMAGE i,IMAGE j)
{
  FMATRIX m;
  
  m = Image2FMatrix(i);
  InverseFMatrix(m);
  FMatrix2Image(m,j);
  DeleteFMatrix(m);
}


/* compute the determinant of a FMATRIX */
static LWFLOAT DetFMatrix(FMATRIX m)
{
  
  int n = m->size;
  LWFLOAT d;
  IVECTOR index= NewIvector(n);
  int i;

  ludcmp(m,index,&d);
  for(i=1;i<=n;i++) d *= m->Y[i][i];
  DeleteIVector(index);
  return(d);
}


/* Matrix inversion using Cholesky Decomposition */
/* The applies only for positive definite symmetric matrices */


void choldc(FMATRIX a, FVECTOR p)
{
  int n = a->size;
  int i,j,k;
  LWFLOAT sum;
  
  for(i=1;i<=n;i++){
    for(j=i;j<=n;j++){
      for(sum = a->Y[i][j],k=i-1;k>=1;k--) 
	sum -= a->Y[i][k]*a->Y[j][k];
      if (i==j){
	if (sum <= 0.0)
	  Errorf("choldc(): Non positive definite matrix");
	p->Y[i] = sqrt(sum);
      }
      else a->Y[j][i] = sum/p->Y[i];
    }
  }
}
	

void cholsl(FMATRIX a, FVECTOR p, FVECTOR b, FVECTOR x)
{
  int n = a->size;
  int i,k;
  LWFLOAT sum;

  for(i=1;i<=n;i++){
    for(sum=b->Y[i], k=i-1;k>=1;k--) sum -= a->Y[i][k]*x->Y[k];
    x->Y[i]=sum/p->Y[i];
  }
  for(i=n;i>=1;i--){
    for(sum=x->Y[i],k=i+1;k<=n;k++) sum -= a->Y[k][i]*x->Y[k];
    x->Y[i] = sum/p->Y[i];
  }
}



static void InversePositiveFMatrix(FMATRIX m) 
{
  int n = m->size;
  FVECTOR col, out, diag;
  FMATRIX res;
  int i,j;

  diag = NewFVector(n);
  col = NewFVector(n);
  out = NewFVector(n);
  res = NewFMatrix(n);
  
  choldc(m,diag);
  for (j=1;j<=n;j++) {
    for (i=1;i<=n;i++) col->Y[i] = 0.0;
    col->Y[j] = 1.0;
    cholsl(m,diag,col,out);
    for(i=1;i<=n;i++) res->Y[i][j] = out->Y[i];
  }
  
  for (j=1;j<=n;j++) 
    for (i=1;i<=n;i++) m->Y[i][j] = res->Y[i][j];