pdfbthmt.cc

matlab官方网站中的用于图像融合技术的contourlet变换源代码

//-----------------------------------------------------------------------------
// pdfbthmt.cc  -  Tying Hidden Markov Tree Models for PDFB
//-----------------------------------------------------------------------------


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <iostream>
#include "pdfbthmt.hh"
#include "utils.hh"
#include "mex.h"

using std::cerr;
using std::endl;

//-----------------------------------------------------------------------------
THMT::THMT(int ns, int nl, int* levndir, bool zm)
    :M(ns), nCh(4), nLev(nl), zeromean(zm),
     model_trans(nLev, vector<matrix<double> >() ),
     model_mean(nLev, vector< vector<double> >() ), 
     model_stdv(nLev, vector< vector<double> >() )
{
    int i, J, k;
    
    for(i=0; i<nLev; i++){
      model_trans[i] = vector< matrix<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
						matrix<double>(M,M));
      model_mean[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
      model_stdv[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
    }

    rnd_init_model();

    // subbandtree is a tree structure used to keep track of which
    // directional subband does each coefficient in a tree belong to
    subbandtree = tree<int> (1, nCh, nLev, 0);
    // setting up the subbandtree tree
    for(J = 0; J<nLev-1; J++)
      for(i=0; i<subbandtree[J].size(); i++) 
	if(levndir[J+1]==levndir[J])
	  for(k=0; k<nCh; k++)
	    subbandtree[J+1][i*nCh+k] = subbandtree[J][i];
	else
	  for(k=0; k<nCh; k++)
	    if (k<nCh/2)
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2;
	    else
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2+1;
}


//-----------------------------------------------------------------------------
THMT::THMT(const mxArray* initmodel, int* levndir)
{
    register int J, MM, m, mm, n, i, k;   
    double* doubleptr;
    mxArray* pointer, *pointer2, *pointer3;
    int numfields, numels, zmlen;
    char* zm;

    numfields = mxGetNumberOfFields(initmodel);
    numels = mxGetNumberOfElements(initmodel);
    if ((numfields != 6) && (numfields != 7))
      mexErrMsgTxt("ERROR: number of fields in struct model is incorrect");

    if (numels != 1)
      mexErrMsgTxt("ERROR: Too many elements");

    if (strcmp(mxGetFieldNameByNumber(initmodel, 0), "nstates") != 0)
      mexErrMsgTxt("Field 0 has wrong name");

    pointer = mxGetFieldByNumber(initmodel, 0, 0);
    doubleptr = mxGetPr(pointer);
    M = (int)(*doubleptr);

    nCh = 4;

    if (strcmp(mxGetFieldNameByNumber(initmodel, 1), "nlevels") != 0)
      mexErrMsgTxt("Field 1 has wrong name");    

    pointer = mxGetFieldByNumber(initmodel, 0, 1);
    doubleptr = mxGetPr(pointer);
    nLev = (int)*doubleptr;

    if (strcmp(mxGetFieldNameByNumber(initmodel, 2), "zeromean") != 0)
      mexErrMsgTxt("Field 2 has wrong name");
    pointer = mxGetFieldByNumber(initmodel, 0, 2);
    zmlen = mxGetM(pointer) * mxGetN(pointer) + 1; 
    zm = (char*)mxCalloc(zmlen, sizeof(char));
    if (mxGetString(pointer, zm, zmlen) != 0)
      mexErrMsgTxt("Error: cannot get zeromean variable");
    if (strcmp(zm, "yes") == 0)
      zeromean = true;
    else if (strcmp(zm, "no") == 0)
      zeromean = false;
    else
      mexErrMsgTxt("Error: Zeromean can only be yes or no");

    model_trans = vector<vector<matrix<double> > >(nLev, 
						  vector<matrix<double> >() );
    model_mean = vector<vector<vector<double> > >(nLev, 
						 vector< vector<double> >() );
    model_stdv = vector<vector<vector<double> > >(nLev, 
						 vector< vector<double> >() );
    for(i=0; i<nLev; i++){
      model_trans[i] = vector< matrix<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
						matrix<double>(M,M));
      model_mean[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
      model_stdv[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
    }

    if (strcmp(mxGetFieldNameByNumber(initmodel, 3), "rootprob") != 0)
      mexErrMsgTxt("Field 3 has wrong name");
    pointer = mxGetFieldByNumber(initmodel, 0, 3);
    if (pointer == NULL) {
      mexPrintf("%s%d\n",
    		"FIELD:", 3);
      mexErrMsgTxt("Above field is empty!"); 
    }
    doubleptr = mxGetPr(pointer);
    for (m = 0; m < M; m++)
         model_trans[0][0][m][0]= doubleptr[m];
 
    if (strcmp(mxGetFieldNameByNumber(initmodel, 4), "transprob") != 0)
      mexErrMsgTxt("Field 4 has wrong name");
    pointer = mxGetFieldByNumber(initmodel, 0, 4);
    if (pointer == NULL) {
      mexPrintf("%s%d\n",
    		"FIELD:", 4);
      mexErrMsgTxt("Above field is empty!"); 
    }

    // Trans probs
    for (J =  1; J < nLev; J++)
    { 
      pointer2 = mxGetCell(pointer,J-1);
      for (m = 0; m < M; m++){
	for (mm = 0; mm < model_trans[J].size(); mm++) {
	    pointer3 = mxGetCell(pointer2, mm);
	    doubleptr = mxGetPr(pointer3);
	    for (n = 0; n < M; n++)
	       model_trans[J][mm][m][n] = doubleptr[n*M+m];
	}
      }
    }

    // Mean
    if (!zeromean)
    {
        if (strcmp(mxGetFieldNameByNumber(initmodel, 5), "mean") != 0)
	  mexErrMsgTxt("Field 5 has wrong name");
        pointer = mxGetFieldByNumber(initmodel, 0, 5);
	if (pointer == NULL) {
	  mexPrintf("%s%d\n",
		    "FIELD:", 5);
	  mexErrMsgTxt("Above field is empty!");
	}
	for (J = 0; J < nLev; J++)
	{
	  pointer2 = mxGetCell(pointer, J);
	  for (mm=0; mm < model_mean[J].size(); mm++){
	    pointer3 = mxGetCell(pointer2, mm);
	    doubleptr = mxGetPr(pointer3);
	    for (m=0; m<M; m++)
	       model_mean[J][mm][m] = doubleptr[m];
	  }
	}
    }

    if(!zeromean){
      if (strcmp(mxGetFieldNameByNumber(initmodel, 6), "stdv") != 0)
	mexErrMsgTxt("Field 6 has wrong name");
      pointer = mxGetFieldByNumber(initmodel, 0, 6);
      if (pointer == NULL) {
	mexPrintf("%s%d\n",
		  "FIELD:", 6);
	mexErrMsgTxt("Above field is empty!");
      }
    }
    else{
      if (strcmp(mxGetFieldNameByNumber(initmodel, 5), "stdv") != 0)
	mexErrMsgTxt("Field 5 has wrong name");
      pointer = mxGetFieldByNumber(initmodel, 0, 5);
      if (pointer == NULL) {
	mexPrintf("%s%d\n",
		  "FIELD:", 5);
	mexErrMsgTxt("Above field is empty!");
      } 
    }

    // Standard deviation
    for (J =  0; J < nLev; J++)
    {
      pointer2 = mxGetCell(pointer, J);
      for (mm=0; mm < model_stdv[J].size(); mm++){
	pointer3 = mxGetCell(pointer2, mm);
	doubleptr = mxGetPr(pointer3);
	for (m = 0; m < M; m++) 
	    model_stdv[J][mm][m] = doubleptr[m];
      }
    }

    subbandtree = tree<int> (1, nCh, nLev, 0);
    // setting up the subbandtree tree
    for(J = 0; J<nLev-1; J++)
      for(i=0; i<subbandtree[J].size(); i++) 
	if(levndir[J+1]==levndir[J])
	  for(k=0; k<nCh; k++)
	    subbandtree[J+1][i*nCh+k] = subbandtree[J][i];
	else
	  for(k=0; k<nCh; k++)
	    if (k<nCh/2)
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2;
	    else
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2+1;
}


//-----------------------------------------------------------------------------
THMT::THMT(char *filename, int* levndir, long& startpos)
{
    FILE *fp;
    register int J, MM, m, mm, n, i, k;
    char szm[10];
   
    fp = fopen(filename,"a+");
    if (!fp) 
    {
	cerr << "ERROR: can not open for reading " << filename << endl;
	exit(-1);
    }

    fseek(fp, startpos, SEEK_SET);

    if (fscanf(fp, "nStates: %d\n", &M) != 1) 
    {
	cerr << "ERROR: problem reading nStates field of " 
	     << filename << endl;
	exit(-1);
    }

    nCh = 4;

    if (fscanf(fp, "nLevels: %d\n", &nLev) != 1) 
    {
	cerr << "ERROR: problem reading nLevels field of " 
	     << filename << endl;
	exit(-1);
    }

    if (fscanf(fp, "zeroMean: %s\n", &szm) != 1) 
    {
	cerr << "ERROR: problem reading zeroMean field of " 
	     << filename << endl;
	exit(-1);
    }

    if (strcmp(szm, "yes") == 0)
	zeromean = true;
    else
	zeromean = false;

    // Allocate space for model parameters
    model_trans = vector<vector<matrix<double> > >(nLev, 
						  vector<matrix<double> >() );
    model_mean = vector<vector<vector<double> > >(nLev, 
						 vector< vector<double> >() );
    model_stdv = vector<vector<vector<double> > >(nLev, 
						 vector< vector<double> >() );
    for(i=0; i<nLev; i++){
      model_trans[i] = vector< matrix<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
						matrix<double>(M,M));
      model_mean[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
      model_stdv[i] = vector< vector<double> >((int)pow(2.0,levndir[i]
							 -levndir[0]), 
					       vector<double>(M));
    }

    // Initial probs
    fscanf(fp, "\n");
    for (m = 0; m < M; m++)
	fscanf(fp, "%lf ", &model_trans[0][0][m][0]);  
    fscanf(fp, "\n\n");
  
    // Trans prob
    for (J = 1; J < nLev; J++) 
    {
      for (m = 0; m < M; m++){
	for (mm = 0; mm < model_trans[J].size(); mm++)
	  {
	    for (n = 0; n < M; n++)
	      fscanf(fp, "%lf ", &model_trans[J][mm][m][n]);
	  }

	fscanf(fp, "\n");
      }
    }
    fscanf(fp, "\n");

    // Mean
    if (zeromean)
    {
        for (J = 0; J < nLev; J++){
	  for (mm=0; mm<model_mean[J].size(); mm++)
	    for (m = 0; m < M; m++) 
		model_mean[J][mm][m] = 0.0;
	}
    }
    else
    {
	for (J = 0; J < nLev; J++)
	{
	  for (mm=0; mm<model_mean[J].size(); mm++)
	    for (m = 0; m < M; m++) 
		fscanf(fp, "%lf ", &model_mean[J][mm][m]);
	    
	  fscanf(fp, "\n");
	}
	fscanf(fp, "\n");
    }

    // Standard deviation
    for (J=0; J < nLev; J++)
    {
      for (mm=0; mm<model_stdv[J].size(); mm++)
	for (m = 0; m < M; m++) 
	    fscanf(fp, "%lf ", &model_stdv[J][mm][m]);
	  
      fscanf(fp, "\n");
    }
    fscanf(fp, "\n");

    startpos = ftell(fp);
   
    fclose(fp);

    subbandtree = tree<int> (1, nCh, nLev, 0);
    // setting up the subbandtree tree
    for(J = 1; J<nLev-1; J++)
      for(i=0; i<subbandtree[J].size(); i++) 
	if(J%2==0)
	  for(k=0; k<nCh; k++)
	    subbandtree[J+1][i*nCh+k] = subbandtree[J][i];
	else
	  for(k=0; k<nCh; k++)
	    if (k<nCh/2)
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2;
	    else
	      subbandtree[J+1][i*nCh+k] = subbandtree[J][i]*2+1;
}


//-----------------------------------------------------------------------------
void THMT::allocate_training()
{
    int i, J, k;

    if (alpha.nlev() == 0)		// avoid re-allocate
      {
	alpha = 
	    tree< vector<double> > (1, nCh, nLev, vector<double>(M));
	beta = beta_par = 
	    tree< vector<double> > (1, nCh, nLev, vector<double>(M));
	state_prob = tree< vector<double> > (nObs, nCh, nLev, 
					     vector<double>(M));
	scaling = tree<double> (1, nCh, nLev);
	sum_prob = sum_mean = sum_stdv = 
	    vector< vector< vector<double> > >(nLev, vector< vector<double> >());

	sum_trans = 
	  vector< vector< matrix<double> > > 
	  (nLev, vector<matrix<double> >());

	for(i=0; i<nLev; i++) {
	    sum_prob[i] = vector< vector<double> >(model_stdv[i].size(), 
						   vector<double>(M));	
	    sum_mean[i] = vector< vector<double> >(model_stdv[i].size(), 
						   vector<double>(M));
	    sum_stdv[i] = vector< vector<double> >(model_stdv[i].size(), 
						   vector<double>(M));
	    sum_trans[i] = vector< matrix<double> >(model_stdv[i].size(), 
						    matrix<double>(M, M)); 
	}
    }    
}  


//-----------------------------------------------------------------------------
void THMT::allocate_testing()
{
    int i, J, k;

    if (beta.nlev() == 0)		// avoid re-allocate
    {
	beta = beta_par = 
	    tree< vector<double> > (1, nCh, nLev, vector<double>(M));
	scaling = tree<double> (1, nCh, nLev);
    }
}


//-----------------------------------------------------------------------------
void THMT::compute_beta(int ob_ind)
{
    register int J, nNode, i, m, mm, n, c;
    register double o;
    register double sum, prod;
  

    // Initialization of beta down-tree
    J = nLev - 1;				// finest scale

    for (i = 0, nNode = beta[J].size(); i < nNode; i++)
    {
	// o_{J,i}^{ob_ind}
	o = (*obs)[J][ob_ind * ipow(nCh,J) + i];

	mm = subbandtree[J][i];

	for (m = 0; m < M; m++) 
	  beta[J][i][m] = compute_g(o, model_mean[J][mm][m], 
				      model_stdv[J][mm][m]);
    } // end initialization

    // Rescale beta
    rescale(J);

    while (J > 0) 
    {
	// Compute $\beta_{i,p(i)}$
	for (i = 0, nNode = beta_par[J].size(); i < nNode; i++) 
	{   
	  mm = subbandtree[J][i];
	  for (m = 0; m < M; m++) 
	    {
	      sum = 0.0;
		
	      for (n = 0; n < M; n++)
		sum += model_trans[J][mm][n][m] * beta[J][i][n];

	      beta_par[J][i][m] = sum;
	    }
	}

	// Compute $\beta_{p(i)}(m)$
	J--;
	for (i = 0, nNode = beta[J].size(); i < nNode; i++) 
	{
	    // o_{J,i}^{ob_ind}
	    o = (*obs)[J][ob_ind * ipow(nCh,J) + i];
	    mm = subbandtree[J][i];

	    for (m = 0; m < M; m++) 
	    {
		prod = compute_g(o, model_mean[J][mm][m], 
				 model_stdv[J][mm][m]);

		for (c = 0; c < nCh; c++)  // look for child of $p(i)$
		    prod *= beta_par[J+1][i*nCh+c][m];

		beta[J][i][m] = prod;
	    }
	}

	// Rescale $\beta_{p(i)}(m)$
	rescale(J);
    }
}


//-----------------------------------------------------------------------------
void THMT::rescale(int J)
{
    register int i, m, nNode = beta[J].size();
    register double sum;

    // Rescale for each node in this level (J)
    for (i = 0; i < nNode; i++) 
    {
	sum = 0.0;

	for (m = 0; m < M; m++) 
	    sum += beta[J][i][m];