rxx3dtps.cpp

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

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

Rxx3DTPS::Rxx3DTPS()
{
	m_iNumCP = 0;

	m_p3D_TPS_CP = NULL;
	m_p3D_Float_CP = NULL;

	// The regularization parameter a positive scalar, 
	// controls the amount of smoothing; the limiting case of m_lfRegular = 0 reduces to exact interpolation.
	m_lfRegular = 0.0;

	MatrixL = NULL;
	MatrixK = NULL;
	MatrixV_X = NULL;
	MatrixV_Y = NULL;
	MatrixV_Z = NULL;

	grid_Deform_X = NULL;
	grid_Deform_Y = NULL;
	grid_Deform_Z = NULL;
}

Rxx3DTPS::~Rxx3DTPS()
{
//	if (m_p3D_TPS_CP)
//		delete m_p3D_TPS_CP;

//	if (m_p3D_Float_CP)
//		delete m_p3D_Float_CP;

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

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

	if (grid_Deform_X)
		delete [] grid_Deform_X;
	if (grid_Deform_Y)
		delete [] grid_Deform_Y;
	if (grid_Deform_Z)
		delete [] grid_Deform_Z;
}

void Rxx3DTPS::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 + 4)*(m_iNumCP + 4)];
	MatrixV_X = new double[(m_iNumCP + 4)*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 + 4; i++)
		for (j = 0; j < m_iNumCP + 4; j++)
			MatrixL[i*(m_iNumCP + 4) + j] = 0.0;

	for (i = 0; i < m_iNumCP + 4; 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_p3D_TPS_CP[i];
			RxVect4D pt_j = m_p3D_TPS_CP[j];
			pt_i.m[3] = pt_j.m[3] = 0.0;
			
			double elen = (pt_i - pt_j).Magnitude();
			
			MatrixL[i*(m_iNumCP + 4) + j] = MatrixL[j*(m_iNumCP + 4) + 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 + 4) + i] = MatrixK[i*m_iNumCP + i] =
		m_lfRegular * (a*a);

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

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

	MatrixV_X[m_iNumCP + 0] = MatrixV_X[m_iNumCP + 1] = MatrixV_X[m_iNumCP + 2] = MatrixV_X[m_iNumCP + 3] = 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 = -m_GRID_3D_W/2; x < m_GRID_3D_W/2; x++)
	{
		for (int y = -m_GRID_3D_H/2; y < m_GRID_3D_H/2; y++)
		{
			for (int z = -m_GRID_3D_S/2; z < m_GRID_3D_S/2; z++)
			{
				double h = MatrixV_X[m_iNumCP + 0] + MatrixV_X[m_iNumCP + 1]*x + MatrixV_X[m_iNumCP + 2]*y + MatrixV_X[m_iNumCP + 3]*z;

				RxVect4D pt_i, pt_cur(x, y, z, 0);
				
				for (i = 0; i < m_iNumCP; i++)
				{
					pt_i = m_p3D_TPS_CP[i];
					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 + m_GRID_3D_W/2) + (y + m_GRID_3D_H/2)*m_GRID_3D_W + (z + m_GRID_3D_S/2)*m_GRID_3D_W*m_GRID_3D_H] = 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 Rxx3DTPS::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 + 4)*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 + 4; i++)
		for (j = 0; j < m_iNumCP + 4; j++)
			MatrixL[i*(m_iNumCP + 4) + j] = 0.0;

	for (i = 0; i < m_iNumCP + 4; 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_p3D_TPS_CP[i];
			RxVect4D pt_j = m_p3D_TPS_CP[j];
			pt_i.m[3] = pt_j.m[3] = 0.0;
			
			double elen = (pt_i - pt_j).Magnitude();
			
			MatrixL[i*(m_iNumCP + 4) + j] = MatrixL[j*(m_iNumCP + 4) + 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 + 4) + i] = MatrixK[i*m_iNumCP + i] =
		m_lfRegular * (a*a);

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

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

	MatrixV_Y[m_iNumCP + 0] = MatrixV_Y[m_iNumCP + 1] = MatrixV_Y[m_iNumCP + 2] = MatrixV_Y[m_iNumCP + 3] = 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 = -m_GRID_3D_W/2; x < m_GRID_3D_W/2; x++)
	{
		for (int y = -m_GRID_3D_H/2; y < m_GRID_3D_H/2; y++)
		{
			for (int z = -m_GRID_3D_S/2; z < m_GRID_3D_S/2; z++)
			{
				double h = MatrixV_Y[m_iNumCP + 0] + MatrixV_Y[m_iNumCP + 1]*x + MatrixV_Y[m_iNumCP + 2]*y + MatrixV_Y[m_iNumCP + 3]*z;

				RxVect4D pt_i, pt_cur(x, y, z, 0);
				
				for (i = 0; i < m_iNumCP; i++)
				{
					pt_i = m_p3D_TPS_CP[i];
					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 + m_GRID_3D_W/2) + (y + m_GRID_3D_H/2)*m_GRID_3D_W + (z + m_GRID_3D_S/2)*m_GRID_3D_W*m_GRID_3D_H] = 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;
}

void Rxx3DTPS::CalculateTPS_Z(void)
{
	// We need at least 3 points to define a plane
	if (m_iNumCP < 3)
		return;
	
	// Allocate the matrix and vector	
	MatrixV_Z = new double[(m_iNumCP + 4)*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 + 4; i++)
		for (j = 0; j < m_iNumCP + 4; j++)
			MatrixL[i*(m_iNumCP + 4) + j] = 0.0;

	for (i = 0; i < m_iNumCP + 4; i++)
			MatrixV_Z[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_p3D_TPS_CP[i];
			RxVect4D pt_j = m_p3D_TPS_CP[j];
			pt_i.m[3] = pt_j.m[3] = 0.0;
			
			double elen = (pt_i - pt_j).Magnitude();
			
			MatrixL[i*(m_iNumCP + 4) + j] = MatrixL[j*(m_iNumCP + 4) + 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 + 4) + i] = MatrixK[i*m_iNumCP + i] =
		m_lfRegular * (a*a);