/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkHomogeneousTransformTest.cxx,v $
  Language:  C++
  Date:      $Date: 2007-03-14 12:37:08 $
  Version:   $Revision: 1.1 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notices for more information.

   This test was contributed by Rupert Brooks, McGill Centre for Intelligent
   Machines, Montreal, Canada.  It is based loosely on itkAffineTransformTest
   but actually tests the output of most methods.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

#define MSG(A)   std::cout<<A<<std::endl
#define ERRMSG(A)   std::cerr<<A<<std::endl  
#define TRUE 1

#include <iostream>

#include "itkHomogeneousTransform.h"
#include "itkAffineTransform.h"
#include "itkImage.h"
#include "vnl/vnl_vector_fixed.h"

typedef  itk::Matrix<double,3,3>   Matrix2DType;
typedef  itk::Vector<double,2>     Vector2DType;
typedef  itk::Point<double,2>     Point2DType;
typedef  itk::Point<double,3>     HomogPoint2DType;
typedef  itk::Matrix<double,4,4>   Matrix3DType;
typedef  itk::Vector<double,3>     Vector3DType;
typedef  itk::Point<double,3>     Point3DType;
typedef  itk::Point<double,4>     HomogPoint3DType;
    
typedef itk::HomogeneousTransform<double,2> Homogeneous2DType;
typedef itk::AffineTransform<double,2> Affine2DType;
typedef itk::HomogeneousTransform<double,3> Homogeneous3DType;
typedef itk::AffineTransform<double,3> Affine3DType;

namespace
{

void PrintPoint2D( const Point2DType & v )
{
  for( unsigned int i=0; i<Point2DType::Dimension; i++)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}
void PrintPoint3D( const Point3DType & v )
{
  for( unsigned int i=0; i<Point3DType::Dimension; i++)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}
void PrintHPoint2D( const HomogPoint2DType & v )
{
  for( unsigned int i=0; i<HomogPoint2DType::Dimension; i++)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}

void PrintHPoint3D( const HomogPoint3DType & v )
{
  for( unsigned int i=0; i<HomogPoint3DType::Dimension; i++)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}
}


int main(int, char *[])
{


    int any = 0;       // Any errors detected in testing?
	int oldany;        // for keeping track of errors on each test
	double tolerance = 1e-12;
	int dim=2;


	// These results were computed by hand in matlab
	double paramvalues2D[8]={1.1, 0.2, -0.1, 0.9, 0.5, 0.6, 0.1, 0.2};
	double jacobianvalues2D[2][8]={{    3.3333,   -7.7778,         0,         0,  
		                              2.2222,         0,  -10.7407,   25.0617},
	                             {	0,         0,    3.3333,   -7.7778,   
									0,    2.2222,    20.0000, -46.6667}};

	double paramvalues3D[15]={1.1, 0.2,    0.3,    -0.1, 0.9,    0.1,    0.12,    -0.2,    1.2, 
		                      0.5, 0.6,    0.7,    0.1, 0.2,   0.3};
	double jacobianvalues3D[3][15]={{ 0.9091,   -2.1212,    2.4242,         0,         0,
		                                   0,         0,         0,         0,    0.6061,
										   0,         0,   -1.4601,    3.4068,   -3.8935},
	                                      {0,         0,         0,    0.9091,   -2.1212,
								      2.4242,         0,         0,         0,         0,
									  0.6061,         0,    1.2672,   -2.9568,    3.3792},
	                                 {     0,         0,         0,         0,         0,
									       0,    0.9091,   -2.1212,    2.4242,         0,
										   0,    0.6061,   -3.5152,    8.2020,   -9.3737}};


    // The tests are divided into two blocks first the 2D tests, then the 3D.  There
	// are 14 tests in each, and of those 14, many have several subparts.  The same methods
	// are tested in 2D and 3D the same way, except of course that they go up a dimension.

	{
	//the 2D tests

    // Variables
    Matrix2DType                   matrix1, matrix2, inverse2, idmatrix, testmatrix, tempmatrix, testmatrix2;
    Point2DType                   point2,inpoint1,inpoint2,outpoint1,outpoint2,outpoint1a,
								outpoint2a, outpoint2b, outpoint3a, outpoint3b, idpoint,point3,
								outpoint4a, outpoint4b,inpointJ,pointJ, temppoint;
    HomogPoint2DType               hpoint1,hpoint2;

	//Various matrices and points used in testing
    matrix1[0][0] = 1;
    matrix1[0][1] = 0.5;
    matrix1[0][2] = 0.25;
    matrix1[1][0] = 0.3;
    matrix1[1][1] = 3;
    matrix1[1][2] = 0.1;
    matrix1[2][0] = 0.3;
    matrix1[2][1] = 0.2;
    matrix1[2][2] = 1;

	matrix2[0][0] = 1;
    matrix2[0][1] = 0.8;
    matrix2[0][2] = 0.1;
    matrix2[1][0] = 0.2;
    matrix2[1][1] = 1.1;
    matrix2[1][2] = 5;
    matrix2[2][0] = 0.2;
    matrix2[2][1] = 0.1;
    matrix2[2][2] = 1;

	inpoint1[0]=3.4;
	inpoint1[1]=-1.1;

	point2[0] = 5;
    point2[1] = 6;
	
	// the point to calc the jacobian at
	inpointJ[0]=2.5;
	inpointJ[1]=-2.5;
	
	
	// the center of the transform to calc jacobian at
	pointJ[0]=1;
	pointJ[1]=1;




   
    /* Create a 2D identity transformation and show its parameters */
	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 1: Identity Transform");
	MSG("-----------------------------------------------------");
	oldany=any;
    Homogeneous2DType::Pointer id2 = Homogeneous2DType::New();
	idmatrix= id2->GetMatrix();
    idpoint = id2->GetCenter();
    std::cout << "Matrix from instantiating an identity transform:"
              << std::endl << idmatrix;
    std::cout << "Center from instantiating an identity transform:"
              << std::endl;
    PrintPoint2D( idpoint );
	id2->SetIdentity();
	testmatrix=(id2->GetMatrix())-idmatrix;
	double sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, either does not initialize to identity, or set identity broken");
	}

	MSG("Test complete "<<any-oldany<<" errors detected.");
    
	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 2: Create from a matrix, check inverse");
	MSG("-----------------------------------------------------");
	oldany=any;
    /* Create and show a simple 2D transform from given parameters */
    Homogeneous2DType::Pointer homog2 = Homogeneous2DType::New();
    homog2->SetMatrix( matrix1 );
    homog2->SetCenter( point2 );
    MSG( "Instantiation of a given 2D transform:" );
    MSG( homog2 );

    inverse2 = homog2->GetInverseMatrix();
    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix1*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 3: Create around center 0");
	MSG("-----------------------------------------------------");
	// this test doesnt do that much, but we use the transform later
	oldany=any;
	Homogeneous2DType::Pointer homog3=Homogeneous2DType::New();
    homog3->SetMatrix( matrix1 );

    MSG( "Instantiated with center 0:" );
    MSG( homog3 );
    inverse2 = homog3->GetInverseMatrix();
    std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	MSG("Test complete "<<any-oldany<<" errors detected.");


	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 4: Compare the two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
	{
	double a,b;
	a=homog2->Metric(homog3);
	b=homog3->Metric(homog2);
	MSG( "Metric 1-2:"<<a );
	MSG( "Metric 2-1:"<<b );
	if(a!=b) {
		MSG("TEST  FAILED: metric not symmetric");
		any++;
	}
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 5: Try changing the transform now");
	MSG("-----------------------------------------------------");
	// basically, does it remember to update the inverse when setting
	// the matrix
	oldany=any;

	homog3->SetMatrix( matrix2 );

    MSG( "Just changed the matrix of this one" );
    MSG( homog3 );
    inverse2 = homog3->GetInverseMatrix();
    std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;

    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix2*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}


	MSG("Test complete "<<any-oldany<<" errors detected.");


	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 6: Transform a point through and back");
	MSG("-----------------------------------------------------");
	// We push a point through the transform, and also calculate it 
	// manually, and compare the results.
	oldany=any;
    outpoint1=homog3->TransformPoint(inpoint1);
	outpoint2=homog3->BackTransformPoint(outpoint1);
	int flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point is not equal to input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint2D(outpoint1);
		MSG("Backtransformed Point: ");
		PrintPoint2D(outpoint2);
	}
	tempmatrix=homog3->GetMatrix();
	MSG("Matrix for homog 3");
	MSG(tempmatrix);
	temppoint =homog3->GetCenter();
	MSG("Center for homog 3");
	MSG(temppoint);
	for (unsigned int i=0; i<dim;i++) {
		hpoint1[i]=inpoint1[i]-temppoint[i];
		hpoint2[i]=temppoint[i];
	}
	hpoint1[dim]=1;
	hpoint2[dim]=0;
	for (unsigned int i=0; i<dim+1;i++) {
	for (unsigned int j=0; j<dim+1;j++) {
		hpoint2[i]+=tempmatrix[i][j]*hpoint1[j];
	}
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		outpoint2[i]=hpoint2[i]/hpoint2[dim];
		if(fabs(outpoint2[i]-outpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, manually computed point is not equal to output");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint2D(outpoint1);
		MSG("Homogeneous Point 1: ");
		PrintHPoint2D(hpoint1);
		MSG("Homogeneous Point 2: ");
		PrintHPoint2D(hpoint2);
		MSG("Manually Calced Point 2: ");
		PrintPoint2D(outpoint2);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 7: Test the decentering method");
	MSG("-----------------------------------------------------");
	// Even when the center is moved around, the way points transform
	// should remain unchanged.
	oldany=any;
    outpoint1=homog3->TransformPoint(inpoint1);
	outpoint2=homog3->BackTransformPoint(outpoint1);
	tempmatrix=homog3->DeCenter();
	MSG("Decentered Matrix for homog 3");
	MSG(tempmatrix);
	for (unsigned int i=0; i<dim;i++) {
		hpoint1[i]=inpoint1[i];
	}
	hpoint1[dim]=1;
	for (unsigned int i=0; i<dim+1;i++) {
	for (unsigned int j=0; j<dim+1;j++) {
		hpoint2[i]+=tempmatrix[i][j]*hpoint1[j];
	}
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		outpoint2[i]=hpoint2[i]/hpoint2[dim];
		if(fabs(outpoint2[i]-outpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, manually computed on decentered matrix is not equal to output");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint2D(outpoint1);
		MSG("Homogeneous Point 1: ");
		PrintHPoint2D(hpoint1);
		MSG("Homogeneous Point 2: ");
		PrintHPoint2D(hpoint2);
		MSG("Manually Calced Point 2: ");
		PrintPoint2D(outpoint2);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");


    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 8: Try recentering");
	MSG("-----------------------------------------------------");
	oldany=any;
	// this has the effect of exercising the Decenter and recenter
	// methods as well
	homog3->ReCenter( point2 );
	MSG("Recentering Matrix:");
	testmatrix=homog3->ReCenter(matrix2,point2);
	MSG(testmatrix);
	MSG("Going back:");
	for (unsigned int i=0;i<dim;i++) {
		point3[i]=-point2[i];
	}
	testmatrix2=homog3->ReCenter(testmatrix,point3);
	MSG(testmatrix2);


	MSG( "Just changed the center to:" );
	PrintPoint2D(point2);

    MSG( homog3 );
    outpoint1a=homog3->TransformPoint(inpoint1);
	outpoint2a=homog3->BackTransformPoint(outpoint1a);
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint1[i]-outpoint1a[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same with new center");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Old Transformed Point: ");
		PrintPoint2D(outpoint1);
		MSG("New transformed Point: ");
		PrintPoint2D(outpoint1a);
	}

	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(inpoint1[i]-outpoint2a[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, backtransformed point is not the same with new center");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Point to backtransform: ");
		PrintPoint2D(outpoint1a);
		MSG("New Backtransformed Point: ");
		PrintPoint2D(outpoint2a);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");
	
	
	/* Try composition of two transformations */
    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 9: Try composing two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
    // homog2 and homog 3 are different at this point.  Pass a point 
	// through and back on both, then on the composition.  result 
	// should be the same
    outpoint1a=homog3->TransformPoint(inpoint1);
	outpoint2a=homog2->TransformPoint(outpoint1a);
	outpoint3a=homog2->BackTransformPoint(outpoint2a);
	outpoint4a=homog3->BackTransformPoint(outpoint3a);
	homog3->Compose(homog2);
	outpoint2b=homog3->TransformPoint(inpoint1);
	outpoint4b=homog3->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint2D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 10: Try backwards composing two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
	// We are doing the same thing, but using the "pre" qualifier to
	// reverse the order of composition
    outpoint1a=homog2->TransformPoint(inpoint1);
	outpoint2a=homog3->TransformPoint(outpoint1a);
	outpoint3a=homog3->BackTransformPoint(outpoint2a);
	outpoint4a=homog2->BackTransformPoint(outpoint3a);
	homog3->Compose(homog2,1);
	outpoint2b=homog3->TransformPoint(inpoint1);
	outpoint4b=homog3->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint2D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	/* Try composition of two transformations */
    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 11: Try composing with a MatrixOffset Transform");
	MSG("-----------------------------------------------------");
	oldany=any;
    // the Homogeneous transform should compose with any Matrix offset
    // transform.  Here we try an affine transform
	Affine2DType::Pointer aff1=Affine2DType::New();
	Affine2DType::ParametersType params=aff1->GetParameters();
	for(unsigned int i=0;i<aff1->GetNumberOfParameters();i++) {
		params[i]+=i*0.1;
	}
	aff1->SetParameters(params);
	MSG("... Affine transform has parameters"<<params);
    outpoint1a=homog2->TransformPoint(inpoint1);
	outpoint2a=aff1->TransformPoint(outpoint1a);
	outpoint3a=aff1->BackTransformPoint(outpoint2a);
	outpoint4a=homog2->BackTransformPoint(outpoint3a);
	MSG("Composed result");
	homog2->Compose(aff1);
	MSG(homog2);
	outpoint2b=homog2->TransformPoint(inpoint1);
	outpoint4b=homog2->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint2D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 2D TEST 12: Try backwards composing with Matrix offset transform");
	MSG("-----------------------------------------------------");
	oldany=any;
	// We are doing the same thing, but using the "pre" qualifier to
	// reverse the order of composition
    outpoint1a=aff1->TransformPoint(inpoint1);
	outpoint2a=homog2->TransformPoint(outpoint1a);
	outpoint3a=homog2->BackTransformPoint(outpoint2a);
	outpoint4a=aff1->BackTransformPoint(outpoint3a);
	homog2->Compose(aff1,TRUE);
	outpoint2b=homog2->TransformPoint(inpoint1);
	outpoint4b=homog2->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint2D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint2D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint2D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 13: Try Get/SetParameters()");
	MSG("-----------------------------------------------------");
	// here we check that the number of parameters is correct, that
	// they line up with what we expect them to in the internal matrix
	// and that when you get them, you get what you set
	// Also - whether or not it remembers to recalculate the inverse
	oldany=any;
	Homogeneous2DType::ParametersType params2=homog2->GetParameters();
	if(homog2->GetNumberOfParameters()!=(dim+1)*(dim+1)-1) {
		any++;
		MSG("TEST FAILED: Get Number of parameters is incorrect");
	}
	testmatrix=homog2->GetMatrix();
	flag=0;
	unsigned int count=0;
	for(unsigned int i=0;i<dim;i++) {
    	for(unsigned int j=0;j<dim;j++) {
			if(testmatrix[i][j]!=params2[count++]) {
				flag++;
			}
		}
		if(testmatrix[i][dim]!=params2[dim*dim+i]) {
			MSG("M["<<i<<"]["<<dim<<"] ("<< testmatrix[i][dim] <<") !=P["<<dim*dim+i<<"] ("<<params2[dim*dim+i]<<")");
			flag++;
		}
		if(testmatrix[dim][i]!=params2[dim*dim+dim+i]) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test FAILED: parameters from get parameters do not agree with matrix");
		MSG("Get parameters: "<<params2);
		MSG("Matrix:");
		MSG(testmatrix);
	}
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		params2[i]+=(i+1)*0.1;
	}
	homog2->SetParameters(params2);
	Homogeneous2DType::ParametersType params3=homog2->GetParameters();
	flag=0;
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		if(params3[i]!=params2[i]) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test FAILED: parameters from get parameters do not agree with ");
		MSG("what was just set");
		MSG("Parameters were just set to: "<<params2);
		MSG("Parameters returned: "<<params3);
	}
	// was the inverse recomputed?
    inverse2 = homog2->GetInverseMatrix();
	matrix2  = homog2->GetMatrix();
    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix2*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 14: Test the Jacobian");
	MSG("-----------------------------------------------------");
	// Test against a Jacobian computed by hand, elsewhere.
	oldany=any;
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		params2[i]=paramvalues2D[i];
	}
    homog2->SetCenter(pointJ);
	homog2->SetParameters(params2);
	Homogeneous2DType::JacobianType jacobian=homog2->GetJacobian(inpointJ);
	flag=0;
	for(unsigned int d=0;d<dim;d++) {
		for (unsigned int p=0;p<homog2->GetNumberOfParameters();p++) {
			if(fabs(jacobian[d][p]-jacobianvalues2D[d][p])>0.0001) {
				flag++;
			}
		}
	}
	if(flag) {
		any++;
		MSG("Jacobian calculation failed");
		MSG("Hand computed jacobian:");
     	for(unsigned int d=0;d<dim;d++) {
		for (unsigned int p=0;p<homog2->GetNumberOfParameters();p++) {
			std::cout<<jacobianvalues2D[d][p]<<"\t";
		}
		std::cout<<std::endl;
		}
		MSG("Transform jacobian:");
		MSG(jacobian);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 2D TEST 15: Test Get/Set Translation");
	MSG("-----------------------------------------------------");
    Vector2DType trans=homog2->GetTranslation();
	tempmatrix=homog2->GetMatrix();
		// Translation should be the upper right column

    flag=0;
	for(unsigned int d=0;d<dim;d++) {
			if(fabs(tempmatrix[d][dim]-trans[d])>0.0001) {
				flag++;
			}
	}
	if(flag) {
		any++;
		MSG("GetTranslation failed()");
		MSG("Matrix:");
		MSG(tempmatrix);
		MSG("Translation reported:");
		MSG(trans);
	}
	for(unsigned int d=0;d<dim;d++) {
       trans[d]=d+1;	
	}
	homog2->SetTranslation(trans);
	tempmatrix=homog2->GetMatrix();
		// Translation should be the upper right column

    flag=0;
	for(unsigned int d=0;d<dim;d++) {
			if(fabs(tempmatrix[d][dim]-trans[d])>0.0001) {
				flag++;
			}
	}
	if(flag) {
		any++;
		MSG("SetTranslation() failed()");
		MSG("Matrix:");
		MSG(tempmatrix);
		MSG("Translation reported:");
		MSG(trans);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

}// end of 2D testing
    dim=3;
	{
	//the 3D tests

    // Variables
    Matrix3DType                   matrix1, matrix2, inverse2, idmatrix, testmatrix, tempmatrix, testmatrix2;
    Point3DType                   point2,inpoint1,inpoint2,outpoint1,outpoint2,outpoint1a,
								outpoint2a, outpoint2b, outpoint3a, outpoint3b, idpoint,point3,
								outpoint4a, outpoint4b,inpointJ,pointJ, temppoint;
    HomogPoint3DType               hpoint1,hpoint2;

	//Various matrices and points used in testing
    matrix1[0][0] = 1;
    matrix1[0][1] = 0.5;
    matrix1[0][2] = 0.25;
    matrix1[0][3] = -0.25;
    matrix1[1][0] = 0.3;
    matrix1[1][1] = 3;
    matrix1[1][2] = 0.1;
    matrix1[1][3] = -0.1;
    matrix1[2][0] = 0.3;
    matrix1[2][1] = 0.2;
    matrix1[2][2] = 1;
    matrix1[2][3] = 0.02;
    matrix1[3][0] = 0.3;
    matrix1[3][1] = 0.2;
    matrix1[3][2] = -0.01;
    matrix1[3][3] = 1;

	matrix2[0][0] = 1;
    matrix2[0][1] = 0.8;
    matrix2[0][2] = 0.1;
    matrix2[0][3] = -0.1;
    matrix2[1][0] = 0.2;
    matrix2[1][1] = 1.1;
    matrix2[1][2] = 0.02;
    matrix2[1][3] = -4.3;
    matrix2[2][0] = 0.2;
    matrix2[2][1] = 0.1;
    matrix2[2][2] = 1;
    matrix2[2][3] = -0.15;
    matrix2[3][0] = 0.2;
    matrix2[3][1] = 0.1;
    matrix2[3][2] = 0.0001;
    matrix2[3][3] = 1;

	inpoint1[0]=3.4;
	inpoint1[1]=-1.1;
	inpoint1[2]=-2.2;

	point2[0] = 5;
    point2[1] = 6;
    point2[2] = 7;
	
	// the point to calc the jacobian at
	inpointJ[0]=2.5;
	inpointJ[1]=-2.5;
	inpointJ[2]=5;
	
	
	// the center of the transform to calc jacobian at
	pointJ[0]=1;
	pointJ[1]=1;
	pointJ[2]=1;




   
    /* Create a 3D identity transformation and show its parameters */
	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 1: Identity Transform");
	MSG("-----------------------------------------------------");
	oldany=any;
    Homogeneous3DType::Pointer id2 = Homogeneous3DType::New();
	idmatrix= id2->GetMatrix();
    idpoint = id2->GetCenter();
    std::cout << "Matrix from instantiating an identity transform:"
              << std::endl << idmatrix;
    std::cout << "Center from instantiating an identity transform:"
              << std::endl;
    PrintPoint3D( idpoint );
	id2->SetIdentity();
	testmatrix=(id2->GetMatrix())-idmatrix;
	double sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, either does not initialize to identity, or set identity broken");
	}

	MSG("Test complete "<<any-oldany<<" errors detected.");
    
	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 2: Create from a matrix, check inverse");
	MSG("-----------------------------------------------------");
	oldany=any;
    /* Create and show a simple 3D transform from given parameters */
    Homogeneous3DType::Pointer homog2 = Homogeneous3DType::New();
    homog2->SetMatrix( matrix1 );
    homog2->SetCenter( point2 );
    MSG( "Instantiation of a given 3D transform:" );
    MSG( homog2 );

    inverse2 = homog2->GetInverseMatrix();
    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix1*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 3: Create around center 0");
	MSG("-----------------------------------------------------");
	// this test doesnt do that much, but we use the transform later
	oldany=any;
	Homogeneous3DType::Pointer homog3=Homogeneous3DType::New();
    homog3->SetMatrix( matrix1 );

    MSG( "Instantiated with center 0:" );
    MSG( homog3 );
    inverse2 = homog3->GetInverseMatrix();
    std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	MSG("Test complete "<<any-oldany<<" errors detected.");


	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 4: Compare the two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
	{
	double a,b;
	a=homog2->Metric(homog3);
	b=homog3->Metric(homog2);
	MSG( "Metric 1-2:"<<a );
	MSG( "Metric 2-1:"<<b );
	if(a!=b) {
		MSG("TEST FAILED: metric not symmetric");
		any++;
	}
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 5: Try changing the transform now");
	MSG("-----------------------------------------------------");
	// basically, does it remember to update the inverse when setting
	// the matrix
	oldany=any;

	homog3->SetMatrix( matrix2 );

    MSG( "Just changed the matrix of this one" );
    MSG( homog3 );
    inverse2 = homog3->GetInverseMatrix();
    std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;

    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix2*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}


	MSG("Test complete "<<any-oldany<<" errors detected.");


	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 6: Transform a point through and back");
	MSG("-----------------------------------------------------");
	// We push a point through the transform, and also calculate it 
	// manually, and compare the results.
	oldany=any;
    outpoint1=homog3->TransformPoint(inpoint1);
	outpoint2=homog3->BackTransformPoint(outpoint1);
	int flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point is not equal to input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint3D(outpoint1);
		MSG("Backtransformed Point: ");
		PrintPoint3D(outpoint2);
	}
	tempmatrix=homog3->GetMatrix();
	MSG("Matrix for homog 3");
	MSG(tempmatrix);
	temppoint =homog3->GetCenter();
	MSG("Center for homog 3");
	MSG(temppoint);
	for (unsigned int i=0; i<dim;i++) {
		hpoint1[i]=inpoint1[i]-temppoint[i];
		hpoint2[i]=temppoint[i];
	}
	hpoint1[dim]=1;
	hpoint2[dim]=0;
	for (unsigned int i=0; i<dim+1;i++) {
	for (unsigned int j=0; j<dim+1;j++) {
		hpoint2[i]+=tempmatrix[i][j]*hpoint1[j];
	}
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		outpoint2[i]=hpoint2[i]/hpoint2[dim];
		if(fabs(outpoint2[i]-outpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, manually computed point is not equal to output");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint3D(outpoint1);
		MSG("Homogeneous Point 1: ");
		PrintHPoint3D(hpoint1);
		MSG("Homogeneous Point 2: ");
		PrintHPoint3D(hpoint2);
		MSG("Manually Calced Point 2: ");
		PrintPoint3D(outpoint2);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 7: Test the decentering method");
	MSG("-----------------------------------------------------");
	// Even when the center is moved around, the way points transform
	// should remain unchanged.
	oldany=any;
    outpoint1=homog3->TransformPoint(inpoint1);
	outpoint2=homog3->BackTransformPoint(outpoint1);
	tempmatrix=homog3->DeCenter();
	MSG("Decentered Matrix for homog 3");
	MSG(tempmatrix);
	for (unsigned int i=0; i<dim;i++) {
		hpoint1[i]=inpoint1[i];
	}
	hpoint1[dim]=1;
	for (unsigned int i=0; i<dim+1;i++) {
	for (unsigned int j=0; j<dim+1;j++) {
		hpoint2[i]+=tempmatrix[i][j]*hpoint1[j];
	}
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		outpoint2[i]=hpoint2[i]/hpoint2[dim];
		if(fabs(outpoint2[i]-outpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, manually computed on decentered matrix is not equal to output");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point: ");
		PrintPoint3D(outpoint1);
		MSG("Homogeneous Point 1: ");
		PrintHPoint3D(hpoint1);
		MSG("Homogeneous Point 2: ");
		PrintHPoint3D(hpoint2);
		MSG("Manually Calced Point 2: ");
		PrintPoint3D(outpoint2);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");


    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 8: Try recentering");
	MSG("-----------------------------------------------------");
	oldany=any;
	// this has the effect of exercising the Decenter and recenter
	// methods as well
	homog3->ReCenter( point2 );
	MSG("Recentering Matrix:");
	testmatrix=homog3->ReCenter(matrix2,point2);
	MSG(testmatrix);
	MSG("Going back:");
	for (unsigned int i=0;i<dim;i++) {
		point3[i]=-point2[i];
	}
	testmatrix2=homog3->ReCenter(testmatrix,point3);
	MSG(testmatrix2);


	MSG( "Just changed the center to:" );
	PrintPoint3D(point2);

    MSG( homog3 );
    outpoint1a=homog3->TransformPoint(inpoint1);
	outpoint2a=homog3->BackTransformPoint(outpoint1a);
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint1[i]-outpoint1a[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same with new center");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Old Transformed Point: ");
		PrintPoint3D(outpoint1);
		MSG("New transformed Point: ");
		PrintPoint3D(outpoint1a);
	}

	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(inpoint1[i]-outpoint2a[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, backtransformed point is not the same with new center");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Point to backtransform: ");
		PrintPoint3D(outpoint1a);
		MSG("New Backtransformed Point: ");
		PrintPoint3D(outpoint2a);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");
	
	
	/* Try composition of two transformations */
    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 9: Try composing two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
    // homog2 and homog 3 are different at this point.  Pass a point 
	// through and back on both, then on the composition.  result 
	// should be the same
    outpoint1a=homog3->TransformPoint(inpoint1);
	outpoint2a=homog2->TransformPoint(outpoint1a);
	outpoint3a=homog2->BackTransformPoint(outpoint2a);
	outpoint4a=homog3->BackTransformPoint(outpoint3a);
	homog3->Compose(homog2);
	outpoint2b=homog3->TransformPoint(inpoint1);
	outpoint4b=homog3->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint3D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 10: Try backwards composing two transforms");
	MSG("-----------------------------------------------------");
	oldany=any;
	// We are doing the same thing, but using the "pre" qualifier to
	// reverse the order of composition
    outpoint1a=homog2->TransformPoint(inpoint1);
	outpoint2a=homog3->TransformPoint(outpoint1a);
	outpoint3a=homog3->BackTransformPoint(outpoint2a);
	outpoint4a=homog2->BackTransformPoint(outpoint3a);
	homog3->Compose(homog2,TRUE);
	outpoint2b=homog3->TransformPoint(inpoint1);
	outpoint4b=homog3->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint3D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	/* Try composition of two transformations */
    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 11: Try composing with a MatrixOffset Transform");
	MSG("-----------------------------------------------------");
	oldany=any;
    // the Homogeneous transform should compose with any Matrix offset
    // transform.  Here we try an affine transform
	Affine3DType::Pointer aff1=Affine3DType::New();
	Affine3DType::ParametersType params=aff1->GetParameters();
	for(unsigned int i=0;i<aff1->GetNumberOfParameters();i++) {
		params[i]+=i*0.1;
	}
	aff1->SetParameters(params);
	MSG("... Affine transform has parameters"<<params);
    outpoint1a=homog2->TransformPoint(inpoint1);
	outpoint2a=aff1->TransformPoint(outpoint1a);
	outpoint3a=aff1->BackTransformPoint(outpoint2a);
	outpoint4a=homog2->BackTransformPoint(outpoint3a);
	MSG("Composed result");
	homog2->Compose(aff1);
	MSG(homog2);
	outpoint2b=homog2->TransformPoint(inpoint1);
	outpoint4b=homog2->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint3D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

    MSG("-----------------------------------------------------");
	MSG(" 3D TEST 12: Try backwards composing with Matrix offset transform");
	MSG("-----------------------------------------------------");
	oldany=any;
	// We are doing the same thing, but using the "pre" qualifier to
	// reverse the order of composition
    outpoint1a=aff1->TransformPoint(inpoint1);
	outpoint2a=homog2->TransformPoint(outpoint1a);
	outpoint3a=homog2->BackTransformPoint(outpoint2a);
	outpoint4a=aff1->BackTransformPoint(outpoint3a);
	homog2->Compose(aff1,TRUE);
	outpoint2b=homog2->TransformPoint(inpoint1);
	outpoint4b=homog2->BackTransformPoint(outpoint2b);

    // So now outpoint2a and 2b should be equal
	// outpoint4a and 4b should be equal to inpoint
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint2a[i]-outpoint2b[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, transformed point is not the same when transform composed");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint2a);
		MSG("Transformed Point composed: ");
		PrintPoint3D(outpoint2b);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4a[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (two transforms) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4a);
	}
	flag=0;
	for (unsigned int i=0; i<dim;i++) {
		if(fabs(outpoint4b[i]-inpoint1[i])>tolerance) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test failed, back transformed point (composed) is not the same as input");
		MSG("Input Point: ");
		PrintPoint3D(inpoint1);
		MSG("Transformed Point two transforms: ");
		PrintPoint3D(outpoint4b);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 13: Try Get/SetParameters()");
	MSG("-----------------------------------------------------");
	// here we check that the number of parameters is correct, that
	// they line up with what we expect them to in the internal matrix
	// and that when you get them, you get what you set
	// Also - whether or not it remembers to recalculate the inverse
	oldany=any;
	Homogeneous3DType::ParametersType params2=homog2->GetParameters();
	if(homog2->GetNumberOfParameters()!=(dim+1)*(dim+1)-1) {
		any++;
		MSG("TEST FAILED: Get Number of parameters is incorrect");
	}
	testmatrix=homog2->GetMatrix();
	flag=0;
	unsigned int count=0;
	for(unsigned int i=0;i<dim;i++) {
    	for(unsigned int j=0;j<dim;j++) {
			if(testmatrix[i][j]!=params2[count++]) {
				flag++;
			}
		}
		if(testmatrix[i][dim]!=params2[dim*dim+i]) {
			MSG("M["<<i<<"]["<<dim<<"] ("<< testmatrix[i][dim] <<") !=P["<<dim*dim+i<<"] ("<<params2[dim*dim+i]<<")");
			flag++;
		}
		if(testmatrix[dim][i]!=params2[dim*dim+dim+i]) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test FAILED: parameters from get parameters do not agree with matrix");
		MSG("Get parameters: "<<params2);
		MSG("Matrix:");
		MSG(testmatrix);
	}
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		params2[i]+=(i+1)*0.1;
	}
	homog2->SetParameters(params2);
	Homogeneous3DType::ParametersType params3=homog2->GetParameters();
	flag=0;
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		if(params3[i]!=params2[i]) {
			flag++;
		}
	}
	if(flag) {
		any++;
		MSG("Test FAILED: parameters from get parameters do not agree with ");
		MSG("what was just set");
		MSG("Parameters were just set to: "<<params2);
		MSG("Parameters returned: "<<params3);
	}
	// was the inverse recomputed?
    inverse2 = homog2->GetInverseMatrix();
	matrix2  = homog2->GetMatrix();
    // try multiplying this inverse by the matrix, to see if it gives the identity
	testmatrix=matrix2*inverse2-idmatrix;
	sum=0.0;
	for (unsigned int i=0;i<dim+1;i++) {
		for (unsigned int j=0;j<dim+1;j++) {
			sum+=testmatrix[i][j];
		}
	}
	if(sum>tolerance) {
		any++;
		MSG("Test failed, inverse times matrix not equal to identity");
        std::cout << "Inverse matrix for the given transform:"
              << std::endl << inverse2;
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 14: Test the Jacobian");
	MSG("-----------------------------------------------------");
	// Test against a Jacobian computed by hand, elsewhere.
	oldany=any;
	for(unsigned int i=0;i<homog2->GetNumberOfParameters();i++) {
		params2[i]=paramvalues3D[i];
	}
    homog2->SetCenter(pointJ);
	homog2->SetParameters(params2);
	Homogeneous3DType::JacobianType jacobian=homog2->GetJacobian(inpointJ);
	flag=0;
	for(unsigned int d=0;d<dim;d++) {
		for (unsigned int p=0;p<homog2->GetNumberOfParameters();p++) {
			if(fabs(jacobian[d][p]-jacobianvalues3D[d][p])>0.0001) {
				flag++;
			}
		}
	}
	if(flag) {
		any++;
		MSG("Jacobian calculation failed");
		MSG("Hand computed jacobian:");
     	for(unsigned int d=0;d<dim;d++) {
		for (unsigned int p=0;p<homog2->GetNumberOfParameters();p++) {
			std::cout<<jacobianvalues3D[d][p]<<"\t";
		}
		std::cout<<std::endl;
		}
		MSG("Transform jacobian:");
		MSG(jacobian);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

	MSG("-----------------------------------------------------");
	MSG(" 3D TEST 15: Test Get/Set Translation");
	MSG("-----------------------------------------------------");
    Vector3DType trans=homog2->GetTranslation();
	tempmatrix=homog2->GetMatrix();
		// Translation should be the upper right column

    flag=0;
	for(unsigned int d=0;d<dim;d++) {
			if(fabs(tempmatrix[d][dim]-trans[d])>0.0001) {
				flag++;
			}
	}
	if(flag) {
		any++;
		MSG("GetTranslation failed()");
		MSG("Matrix:");
		MSG(tempmatrix);
		MSG("Translation reported:");
		MSG(trans);
	}
	for(unsigned int d=0;d<dim;d++) {
       trans[d]=d+1;	
	}
	homog2->SetTranslation(trans);
	tempmatrix=homog2->GetMatrix();
		// Translation should be the upper right column

    flag=0;
	for(unsigned int d=0;d<dim;d++) {
			if(fabs(tempmatrix[d][dim]-trans[d])>0.0001) {
				flag++;
			}
	}
	if(flag) {
		any++;
		MSG("SetTranslation() failed()");
		MSG("Matrix:");
		MSG(tempmatrix);
		MSG("Translation reported:");
		MSG(trans);
	}
	MSG("Test complete "<<any-oldany<<" errors detected.");

}// end of 3D testing

    MSG("-------------------------------------------------");
    MSG("Total of "<<any<<" errors detected");

    return any;
}