rxx3dtps.cpp

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

		// 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_Z[i] = m_p3D_Float_CP[i].m[2] - m_p3D_TPS_CP[i].m[2];

	MatrixV_Z[m_iNumCP + 0] = MatrixV_Z[m_iNumCP + 1] = MatrixV_Z[m_iNumCP + 2] = MatrixV_Z[m_iNumCP + 3] = 0.0;
	
	// Solve the linear system "inplace"
	if (0 != LU_Solve(MatrixL, MatrixV_Z))
	{
		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_Z[m_iNumCP + 0] + MatrixV_Z[m_iNumCP + 1]*x + MatrixV_Z[m_iNumCP + 2]*y + MatrixV_Z[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_Z[i + 0] * TPS_Base(dev);
				}
				
				grid_Deform_Z[(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_Z[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 Rxx3DTPS::TPS_Base(double elen)
{
	return 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 Rxx3DTPS::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 + 4, n = m_iNumCP + 4;
	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 Rxx3DTPS::AddControlPoint(double RefX, double RefY, double RefZ, double FloatX, double FloatY, double FloatZ)
{
	m_p3D_TPS_CP[m_iNumCP].m[0] = RefX;
	m_p3D_TPS_CP[m_iNumCP].m[1] = RefY;
	m_p3D_TPS_CP[m_iNumCP].m[2] = RefZ;
	m_p3D_TPS_CP[m_iNumCP].m[3] = 1.0;

	m_p3D_Float_CP[m_iNumCP].m[0] = FloatX;
	m_p3D_Float_CP[m_iNumCP].m[1] = FloatY;
	m_p3D_Float_CP[m_iNumCP].m[2] = FloatZ;
	m_p3D_Float_CP[m_iNumCP].m[3] = 1.0;

	m_iNumCP++;
}

void Rxx3DTPS::CalTPS(void)
{
	CalculateTPS_X();
	CalculateTPS_Y();
	CalculateTPS_Z();
}

double Rxx3DTPS::GetDeformX(int x, int y, int z)
{
	return (double)grid_Deform_X[x + y*m_GRID_3D_W + z*m_GRID_3D_W*m_GRID_3D_H];
}

double Rxx3DTPS::GetDeformY(int x, int y, int z)
{
	return (double)grid_Deform_Y[x + y*m_GRID_3D_W + z*m_GRID_3D_W*m_GRID_3D_H];	
}

double Rxx3DTPS::GetDeformZ(int x, int y, int z)
{
	return (double)grid_Deform_Z[x + y*m_GRID_3D_W + z*m_GRID_3D_W*m_GRID_3D_H];
}

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

	x = x - m_GRID_3D_W/2;
	y = y - m_GRID_3D_H/2;
	z = z - m_GRID_3D_S/2;

	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);
	}
	
	return h;
}

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

	x = x - m_GRID_3D_W/2;
	y = y - m_GRID_3D_H/2;
	z = z - m_GRID_3D_S/2;

	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);
	}
	
	return h;
}

double Rxx3DTPS::GetDeformDetailZ(double x, double y, double z)
{
	int i;

	x = x - m_GRID_3D_W/2;
	y = y - m_GRID_3D_H/2;
	z = z - m_GRID_3D_S/2;

	double h = MatrixV_Z[m_iNumCP + 0] + MatrixV_Z[m_iNumCP + 1]*x + MatrixV_Z[m_iNumCP + 2]*y + MatrixV_Z[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_Z[i + 0] * TPS_Base(dev);
	}
	
	return h;
}

void Rxx3DTPS::Initialize(int Grid_X, int Grid_Y, int Grid_Z, int Num_CP)
{
	m_iNumCP = 0;

	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;
	
	m_GRID_3D_W = Grid_X;
	m_GRID_3D_H = Grid_Y;
	m_GRID_3D_S = Grid_Z;

	m_p3D_TPS_CP = new RxVect4D[Num_CP];
	m_p3D_Float_CP = new RxVect4D[Num_CP];
	grid_Deform_X = new float[m_GRID_3D_W*m_GRID_3D_H*m_GRID_3D_S];
	grid_Deform_Y = new float[m_GRID_3D_W*m_GRID_3D_H*m_GRID_3D_S];
	grid_Deform_Z = new float[m_GRID_3D_W*m_GRID_3D_H*m_GRID_3D_S];
}