gistfile1.txt

#!/usr/bin/python

## rigidtransformation.py
#
# This file contains both, the functions to compute the rigid transformation
# between two sets of points and an illustrative example on how does all it works.
# This rigid transformation calculations are only concerned to the rotation and
# translation transformations, they do not consider the scaling transformations.
#
# This implementation is based on [1] and it works like the following:
#
#   1. Create a set of points defining a cube
#   2. Insert this data into a vtk data struture defining a source set
#   3. Apply a rotation and a tanslation on the source set over each axis
#   4. Create a second set of points from the first set transformed. Note that
#      we have added noise to this second set
#   5. Create a second set of points from the first set transformed
#   6. Apply the method presented in [1]
#   7. Create a third set of points from the second set of points and applying the
#      rigid transformation found in (6)
#   8. Display all the sets of points in the screen, second and third sets must to be
#      similar sets.
#
# -References:
#
#     1. Challis JH. A procedure for determining rigid body transformation parameters.
#        Journal of Biomechanics. 1995;28(6):733-737. Available at:
#	     http://www.sciencedirect.com/science/article/B6T82-42JYWR6-9/2/57cc64e204fcd9c483932e9281bc78a6
#
#
# @author Rafael Palomar
#

##########################
# EXTERNAL LIBRARIES
##########################
import numpy as np
from scipy import linalg, mat, dot
from vtk import *
from random import random, choice, seed

##########################
# ENTRY POINT
##########################
if __name__ == "__main__":

	#Create the first set of points	
	pointsSetData1 = list()
	pointsSetData1.append(np.array([100.0, 100.0, -100.0]))
	pointsSetData1.append(np.array([100.0, -100.0, -100.0]))
	pointsSetData1.append(np.array([-100.0, -100.0, -100.0]))
	pointsSetData1.append(np.array([-100.0, 100.0, -100.0]))
	pointsSetData1.append(np.array([100.0, 100.0, 100.0]))
	pointsSetData1.append(np.array([100.0, -100.0, 100.0]))
	pointsSetData1.append(np.array([-100.0, -100.0, 100.0]))
	pointsSetData1.append(np.array([-100.0, 100.0, 100.0]))
	pointsSet1 = np.array(pointsSetData1)

	#Insert the point coordinates  as points into a polyData set
	points = vtkPoints()
	for i in range(len(pointsSet1)):
			points.InsertNextPoint(pointsSet1[i])
	polyData1 = vtkPolyData()
	polyData1.SetPoints(points)

	#Trasnform the whole polyData set
	transformation = vtkTransform()
	transformation.Translate(103,56,10)
	transformation.RotateY(60)
	transformation.RotateX(12)
	transformation.RotateZ(23)
	transformFilter = vtkTransformFilter()
	transformFilter.SetInput(polyData1)
	transformFilter.SetTransform(transformation)
	transformFilter.Update()
	
	#Get the second set of points from the transformation
	seed(0)
	addNoise = lambda vector: [choice([-1,1])*random()+i for i in vector]

	pointsSet2 = np.array([transformFilter.GetOutput().GetPoint(i) for i in range(len(pointsSet1))])
	noisyPointsSet2 = np.array([addNoise(transformFilter.GetOutput().GetPoint(i)) for i in range(len(pointsSet2))])
	#noisyPointsSet2 = np.array([transformFilter.GetOutput().GetPoint(i) for i in range(len(pointsSet2))])

	#Compute the centroid for the two sets of points
	centroid1 = np.mean(pointsSet1, axis=0)      # { (6) in [1] }
	centroid2 = np.mean(noisyPointsSet2, axis=0) # { (7) in [1] }

	#Compute the difference sets for the two sets of points
	differenceSet1 = pointsSet1 - centroid1       # { (10) in [1] }
	differenceSet2 = noisyPointsSet2 - centroid2  # { (11) in [1] }

	#Compute the cross-dispersion matrix (correlation matrix) #{ (19) in [1] }
	correlationMatrix = np.zeros((3,3))
	for i in range(len(differenceSet1)):
		correlationMatrix += np.outer(differenceSet2[i],differenceSet1[i])

	#Perform Singular Value Decomposition on the correlation matrix { (20) in [1] }
	(U,S,V) = linalg.svd(correlationMatrix)
	X = np.dot(U,V) # { (23) in [1] }

	#Get the Rotation matrix
	if abs(linalg.det(X) - 1) < 1e-10:
		rotationMatrix = X
	else:
		 opmatrix = np.hstack((np.zeros((3,2)),np.array([[-1]]*3)))
		 rotationMatrix = np.dot((V*opmatrix),U.transpose())

	#Get the translation vector
	translation = centroid2-np.dot(rotationMatrix,centroid1)
	
	#Append the translation to the rotation matrix to get a 4x4 matrix
	rotationMatrix = np.vstack((np.hstack((rotationMatrix,translation.reshape(3,1))),[0,0,0,1]))
	rigidTransformation = vtkTransform()
	rigidTransformation.SetMatrix([i for i in rotationMatrix.flat])
		
	#Insert the point coordinates  as points into a polyData set
	points2 = vtkPoints()
	for i in range(len(noisyPointsSet2)):
			points2.InsertNextPoint(noisyPointsSet2[i])
	polyData2 = vtkPolyData()
	polyData2.SetPoints(points2)

	#Trasnform the whole polyData set
	transformFilter2 = vtkTransformFilter()
	transformFilter2.SetInput(polyData1)
	transformFilter2.SetTransform(rigidTransformation)
	transformFilter2.Update()

	#Get the third set of points from the inverse
	pointsSet3 = np.array([transformFilter2.GetOutput().GetPoint(i) for i in range(len(pointsSet1))])

	#Print the results
	print "Original:"
	print pointsSet1
	print "Transformed:"
	print noisyPointsSet2
	print "Rigid transformation"
	print rigidTransformation
	print "Recovered by rigid transformation:"
	print pointsSet3