rxx2dtps.cpp

3D reconstruction, medical image processing from colons, using intel image processing for based clas

#include "stdafx.h"
#include "Rxx2DTPS.h"
#include "fusionglobal.h"
#include <math.h>
#include <fstream.h>

Rxx2DTPS::Rxx2DTPS()
{
	m_iNumCP = 0;

	m_p2D_TPS_CP = NULL;
	m_p2D_TPS_CP = new RxVect4D[100];

	m_lfRegular = 0.5;

	MatrixL = NULL;
	MatrixK = NULL;
	MatrixV_X = NULL;
	MatrixV_Y = NULL;
}

Rxx2DTPS::~Rxx2DTPS()
{
	if (m_p2D_TPS_CP)
		delete m_p2D_TPS_CP;

	if (MatrixL)
		delete [] MatrixL;
	if (MatrixK)
		delete [] MatrixK;

	if (MatrixV_X)
		delete [] MatrixV_X;
	if (MatrixV_Y)
		delete [] MatrixV_Y;
}

void Rxx2DTPS::CalculateTPS_X(void)
{
	// We need at least 3 points to define a plane
	if (m_iNumCP < 3)
		return;
	
	// Allocate the matrix and vector	
	MatrixL = new double[(m_iNumCP + 3)*(m_iNumCP + 3)];
	MatrixV_X = new double[(m_iNumCP + 3)*1];
	MatrixK = new double[m_iNumCP*m_iNumCP];

	// Fill K (p x p, upper left of L) and calculate
	// mean edge length from control points
	//
	// K is symmetrical so we really have to
	// calculate only about half of the coefficients.
	int i, j;
	double a = 0.0;

	for (i = 0; i < m_iNumCP + 3; i++)
		for (j = 0; j < m_iNumCP + 3; j++)
			MatrixL[i*(m_iNumCP + 3) + j] = 0.0;

	for (i = 0; i < m_iNumCP + 3; i++)
			MatrixV_X[i] = 0.0;
		
	for (i = 0; i < m_iNumCP; i++)
		for (j = 0; j < m_iNumCP; j++)
			MatrixK[i*(m_iNumCP) + j] = 0.0;

	for (i = 0; i < m_iNumCP; i++)
	{
		for (j = i + 1; j < m_iNumCP; j++)
		{
			RxVect4D pt_i = m_p2D_TPS_CP[i];
			RxVect4D pt_j = m_p2D_TPS_CP[j];
			pt_i.m[2] = pt_j.m[2] = 0.0;
			pt_i.m[3] = pt_j.m[3] = 0.0;
			
			double elen = (pt_i - pt_j).Magnitude();
			
			MatrixL[i*(m_iNumCP + 3) + j] = MatrixL[j*(m_iNumCP + 3) + i] = 
				MatrixK[i*m_iNumCP + j] = MatrixK[j*m_iNumCP + i] =
				TPS_Base(elen);
			a += elen * 2; // same for upper & lower tri
		}
	}
	a /= (double)(m_iNumCP*m_iNumCP);

	// Fill the rest of L
	for (i = 0; i < m_iNumCP; i++)
	{
		// diagonal: reqularization parameters (lambda * a^2)
		MatrixL[i*(m_iNumCP + 3) + i] = MatrixK[i*m_iNumCP + i] =
		m_lfRegular * (a*a);

		// P (p x 3, upper right)
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP] = 1.0;
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP + 1] = m_p2D_TPS_CP[i].m[0];
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP + 2] = m_p2D_TPS_CP[i].m[1];

		// P transposed (3 x p, bottom left)
		MatrixL[(m_iNumCP + 0)*(m_iNumCP + 3) + i] = 1.0;
		MatrixL[(m_iNumCP + 1)*(m_iNumCP + 3) + i] = m_p2D_TPS_CP[i].m[0];
		MatrixL[(m_iNumCP + 2)*(m_iNumCP + 3) + i] = m_p2D_TPS_CP[i].m[1];
	}
	
	// O (3 x 3, lower right)
	for (i = m_iNumCP; i < m_iNumCP + 3; i++)
		for (j = m_iNumCP; j < m_iNumCP + 3; j++)
			MatrixL[i*(m_iNumCP + 3) + j] = 0.0;
	
	// Fill the right hand vector V
	for (i = 0; i < m_iNumCP; i++)
		MatrixV_X[i] = m_p2D_TPS_CP[i].m[2];

	MatrixV_X[m_iNumCP + 0] = MatrixV_X[m_iNumCP + 1] = MatrixV_X[m_iNumCP + 2] = 0.0;
	
	// Solve the linear system "inplace"
	if (0 != LU_Solve(MatrixL, MatrixV_X))
	{
		puts( "Singular matrix! Aborting." );
		exit(1);
	}

	// Interpolate grid heights
	for (int x = -GRID_2D_W/2; x < GRID_2D_W/2; x++)
	{
		for (int y = -GRID_2D_H/2; y < GRID_2D_H/2; y++)
		{
			double h = MatrixV_X[m_iNumCP + 0] + MatrixV_X[m_iNumCP + 1]*x + MatrixV_X[m_iNumCP + 2]*y;

			RxVect4D pt_i, pt_cur(x,y,0,0);
			
			for (i = 0; i < m_iNumCP; i++)
			{
				pt_i = m_p2D_TPS_CP[i];
				pt_i.m[2] = 0.0;
				pt_i.m[3] = 0.0;

				double dev = (pt_i - pt_cur).Magnitude();
				h += MatrixV_X[i + 0] * TPS_Base(dev);
			}
			
			grid_Deform_X[x + GRID_2D_W/2][y + GRID_2D_H/2] = h;
		}
	}

	// Calc bending energy
	double* w = new double[m_iNumCP];
	double* wT = new double[m_iNumCP];
	double sum = 0.0, bending_energy;
	
	for (i = 0; i < m_iNumCP; i++)
		wT[i] = w[i] = MatrixV_X[i];
	
	for (i = 0; i < m_iNumCP; i++)
	{
		sum = 0.0;

		for (j = 0; j < m_iNumCP; j++)
			sum += w[j]*MatrixK[j*m_iNumCP + i];
		
		wT[i] = sum;
	}

	sum = 0.0;
	for (i = 0; i < m_iNumCP; i++)
		sum += wT[i]*w[i];
	
	bending_energy = sum;

	delete [] w;
	delete [] wT;
}

void Rxx2DTPS::CalculateTPS_Y(void)
{
	// We need at least 3 points to define a plane
	if (m_iNumCP < 3)
		return;
	
	// Allocate the matrix and vector	
	MatrixV_Y = new double[(m_iNumCP + 3)*1];

	// Fill K (p x p, upper left of L) and calculate
	// mean edge length from control points
	//
	// K is symmetrical so we really have to
	// calculate only about half of the coefficients.
	int i, j;
	double a = 0.0;

	for (i = 0; i < m_iNumCP + 3; i++)
		for (j = 0; j < m_iNumCP + 3; j++)
			MatrixL[i*(m_iNumCP + 3) + j] = 0.0;

	for (i = 0; i < m_iNumCP + 3; i++)
			MatrixV_Y[i] = 0.0;
		
	for (i = 0; i < m_iNumCP; i++)
		for (j = 0; j < m_iNumCP; j++)
			MatrixK[i*(m_iNumCP) + j] = 0.0;

	for (i = 0; i < m_iNumCP; i++)
	{
		for (j = i + 1; j < m_iNumCP; j++)
		{
			RxVect4D pt_i = m_p2D_TPS_CP[i];
			RxVect4D pt_j = m_p2D_TPS_CP[j];
			pt_i.m[2] = pt_j.m[2] = 0.0;
			pt_i.m[3] = pt_j.m[3] = 0.0;
			
			double elen = (pt_i - pt_j).Magnitude();
			
			MatrixL[i*(m_iNumCP + 3) + j] = MatrixL[j*(m_iNumCP + 3) + i] = 
				MatrixK[i*m_iNumCP + j] = MatrixK[j*m_iNumCP + i] =
				TPS_Base(elen);
			a += elen * 2; // same for upper & lower tri
		}
	}
	a /= (double)(m_iNumCP*m_iNumCP);

	// Fill the rest of L
	for (i = 0; i < m_iNumCP; i++)
	{
		// diagonal: reqularization parameters (lambda * a^2)
		MatrixL[i*(m_iNumCP + 3) + i] = MatrixK[i*m_iNumCP + i] =
		m_lfRegular * (a*a);

		// P (p x 3, upper right)
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP] = 1.0;
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP + 1] = m_p2D_TPS_CP[i].m[0];
		MatrixL[i*(m_iNumCP + 3) + m_iNumCP + 2] = m_p2D_TPS_CP[i].m[1];

		// P transposed (3 x p, bottom left)
		MatrixL[(m_iNumCP + 0)*(m_iNumCP + 3) + i] = 1.0;
		MatrixL[(m_iNumCP + 1)*(m_iNumCP + 3) + i] = m_p2D_TPS_CP[i].m[0];
		MatrixL[(m_iNumCP + 2)*(m_iNumCP + 3) + i] = m_p2D_TPS_CP[i].m[1];
	}
	
	// O (3 x 3, lower right)
	for (i = m_iNumCP; i < m_iNumCP + 3; i++)
		for (j = m_iNumCP; j < m_iNumCP + 3; j++)
			MatrixL[i*(m_iNumCP + 3) + j] = 0.0;
	
	// Fill the right hand vector V
	for (i = 0; i < m_iNumCP; i++)
		MatrixV_Y[i] = m_p2D_TPS_CP[i].m[3];

	MatrixV_Y[m_iNumCP + 0] = MatrixV_Y[m_iNumCP + 1] = MatrixV_Y[m_iNumCP + 2] = 0.0;
	
	// Solve the linear system "inplace"
	if (0 != LU_Solve(MatrixL, MatrixV_Y))
	{
		puts( "Singular matrix! Aborting." );
		exit(1);
	}

	// Interpolate grid heights
	for (int x = -GRID_2D_W/2; x < GRID_2D_W/2; x++)
	{
		for (int y = -GRID_2D_H/2; y < GRID_2D_H/2; y++)
		{
			double h = MatrixV_Y[m_iNumCP + 0] + MatrixV_Y[m_iNumCP + 1]*x + MatrixV_Y[m_iNumCP + 2]*y;

			RxVect4D pt_i, pt_cur(x,y,0,0);
			
			for (i = 0; i < m_iNumCP; i++)
			{
				pt_i = m_p2D_TPS_CP[i];
				pt_i.m[2] = 0.0;
				pt_i.m[3] = 0.0;

				double dev = (pt_i - pt_cur).Magnitude();
				h += MatrixV_Y[i + 0] * TPS_Base(dev);
			}
			
			grid_Deform_Y[x + GRID_2D_W/2][y + GRID_2D_H/2] = h;
		}
	}

	// Calc bending energy
	double* w = new double[m_iNumCP];
	double* wT = new double[m_iNumCP];
	double sum = 0.0, bending_energy;
	
	for (i = 0; i < m_iNumCP; i++)
		wT[i] = w[i] = MatrixV_Y[i];
	
	for (i = 0; i < m_iNumCP; i++)
	{
		sum = 0.0;

		for (j = 0; j < m_iNumCP; j++)
			sum += w[j]*MatrixK[j*m_iNumCP + i];
		
		wT[i] = sum;
	}

	sum = 0.0;
	for (i = 0; i < m_iNumCP; i++)
		sum += wT[i]*w[i];
	
	bending_energy = sum;

	delete [] w;
	delete [] wT;
}

double Rxx2DTPS::TPS_Base(double elen)
{
	if (elen == 0.0)
		return 0.0;
	else
		return elen*elen*log(elen);
}

// Solve a linear equation system a*x=b using inplace LU decomposition.
//
// Stores x in 'b' and overwrites 'a' (with a pivotted LUD).
//
// Matrix 'b' may have any (>0) number of columns but
// must contain as many rows as 'a'.
//
// Possible return values:
//  0=success
//  1=singular matrix
//  2=a.rows != b.rows

int Rxx2DTPS::LU_Solve(double* a, double* b)
{
	// This routine is originally based on the public domain draft for JAMA,
	// Java matrix package available at http://math.nist.gov/javanumerics/jama/

//	if (a.size1() != b.size1())
//		return 2;

	int m = m_iNumCP + 3, n = m_iNumCP + 3;
	int pivsign;
	int* piv = new int[m];

	int i, j, k, z;

	double temp;

	// PART 1: DECOMPOSITION
	//
	// For an m-by-n matrix A with m >= n, the LU decomposition is an m-by-n
	// unit lower triangular matrix L, an n-by-n upper triangular matrix U,
	// and a permutation vector piv of length m so that A(piv,:) = L*U.
	// If m < n, then L is m-by-m and U is m-by-n.

    // Use a "left-looking", dot-product, Crout/Doolittle algorithm.
    for (i = 0; i < m; i++)
		piv[i] = i;
    pivsign = 1;

	// Make a copy of the j-th column to localize references.
	double* LUcolj = new double[m];

    // Outer loop.
    for (j = 0; j < n; j++)
    {
		for (i = 0; i < m; i++)
			LUcolj[i] = a[i*n + j];

		// Apply previous transformations.
		for (i = 0; i < m; i++)
		{
			double* LUrowi = &a[i*n];
			
			// This dot product is very expensive.
			// Optimize for SSE2?
			int kmax = (i<=j)? i:j;
			double sum = 0.0;
            for (k = 0; k < kmax; k++) 
	            sum += LUrowi[k]*LUcolj[k];

			LUrowi[j] = LUcolj[i] -= sum;
		}

		// Find pivot and exchange if necessary.
		//
		// Slightly optimized version of:
		//  for (int i = j+1; i < m; ++i)
		//    if ( fabs(LUcolj[i]) > fabs(LUcolj[p]) )
		//      p = i;
		int p = j;

		for (int i = j + 1; i < m; i++)
			if (fabs(LUcolj[i]) > fabs(LUcolj[p]))
				p = i;

		if (p != j)
		{
			for (k = 0; k < n; k++)
			{
				temp = a[p*n + k];
				a[p*n + k] = a[j*n + k];
				a[j*n + k] = temp;
			}

			temp = piv[p];
			piv[p] = piv[j];
			piv[j] = temp;
			pivsign = -pivsign;
		}

		// Compute multipliers.
		if (j < m && fabs(a[j*n + j]) > 0.0000001)
			for (i = j + 1; i < m; i++)
				a[i*n + j] /= a[j*n + j];
	}

	delete [] LUcolj;

	// PART 2: SOLVE

	// Check singluarity
	for (j = 0; j < n; j++)
		if (fabs(a[j*n + j]) < 0.0000001)
			return 1;

	// Reorder b according to pivotting
	for (i = 0; i < m; i++)
	{
		if (piv[i] != i)
		{
			temp = b[i];
			b[i] = b[piv[i]];
			b[piv[i]] = temp;
			
			for (j = i; j < m; j++)
				if (piv[j] == i )
				{
					piv[j] = piv[i];
					break;
				}
		}
	}

	// Solve L*Y = B(piv,:)
	for (k = 0; k < n; k++)
	{
		for (i = k + 1; i < n; i++)
		{
			b[i] -= b[k] * a[i*n + k];
		}
	}

	// Solve U*X = Y;
	for (k = n - 1; k >= 0; k--)
	{
		b[k] *= 1.0/a[k*n + k];

		for (i = 0; i < k; i++)
		{
			b[i] -= b[k] * a[i*n + k];
		}
	}

	delete [] piv;

	return 0;
}

void Rxx2DTPS::AddControlPoint(double RefX, double RefY, double FloatX, double FloatY)
{
	m_p2D_TPS_CP[m_iNumCP].m[0] = RefX;
	m_p2D_TPS_CP[m_iNumCP].m[1] = RefY;
	m_p2D_TPS_CP[m_iNumCP].m[2] = FloatX - RefX;
	m_p2D_TPS_CP[m_iNumCP].m[3] = FloatY - RefY;

	m_iNumCP++;
}

void Rxx2DTPS::CalTPS(void)
{
	CalculateTPS_X();
	CalculateTPS_Y();
}

double Rxx2DTPS::GetDeformX(int x, int y)
{
	return (double)grid_Deform_X[x][y];
}

double Rxx2DTPS::GetDeformY(int x, int y)
{
	return (double)grid_Deform_Y[x][y];	
}

double Rxx2DTPS::GetDeformDetailX(double x, double y)
{
	int i;

	x = x - GRID_2D_W/2;
	y = y - GRID_2D_H/2;

	double h = MatrixV_X[m_iNumCP + 0] + MatrixV_X[m_iNumCP + 1]*x + MatrixV_X[m_iNumCP + 2]*y;

	RxVect4D pt_i, pt_cur(x,y,0,0);
			
	for (i = 0; i < m_iNumCP; i++)
	{
		pt_i = m_p2D_TPS_CP[i];
		pt_i.m[2] = 0.0;
		pt_i.m[3] = 0.0;

		double dev = (pt_i - pt_cur).Magnitude();
		h += MatrixV_X[i + 0] * TPS_Base(dev);
	}
	
	return h;
}

double Rxx2DTPS::GetDeformDetailY(double x, double y)
{
	int i;

	x = x - GRID_2D_W/2;
	y = y - GRID_2D_H/2;

	double h = MatrixV_Y[m_iNumCP + 0] + MatrixV_Y[m_iNumCP + 1]*x + MatrixV_Y[m_iNumCP + 2]*y;

	RxVect4D pt_i, pt_cur(x,y,0,0);
			
	for (i = 0; i < m_iNumCP; i++)
	{
		pt_i = m_p2D_TPS_CP[i];
		pt_i.m[2] = 0.0;
		pt_i.m[3] = 0.0;

		double dev = (pt_i - pt_cur).Magnitude();
		h += MatrixV_Y[i + 0] * TPS_Base(dev);
	}
	
	return h;
}