Registration1.java

package edu.stanford.rsl.tutorial.dmip;

import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
import edu.stanford.rsl.conrad.numerics.SimpleMatrix.InversionType;
import edu.stanford.rsl.conrad.numerics.SimpleOperators;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
import ij.gui.Plot;


/**
 * Exercise 7 of Diagnostic Medical Image Processing (DMIP)
 * @author Bastian Bier
 *
 */
public class Registration1 {
	
	/**
	 * Registration method
	 * Point Correspondences are used to calculate the rotation and translation
	 * Transformation from p to q
	 * @param p reference point cloud
	 * @param q point cloud to be registered
	 * @return translation and rotation for the registration (phi, t1, t2)
	 */
	private SimpleVector registrationUsingPointCorrespondences(SimpleMatrix p, SimpleMatrix q){
		
		int numPoints = p.getRows();
		
		// Build up measurement matrix m
		SimpleMatrix m = new SimpleMatrix(numPoints * 2, 4);
		
		for(int i = 0; i < numPoints * 2; i++)
		{
			if(i < numPoints)
			{
				// TODO
				// TODO
				// TODO
				// TODO
			}
			if(i >= numPoints)
			{
				// TODO
				// TODO
				// TODO
				// TODO
			}
		}
		
		// Build up solution vector b
		SimpleVector b = new SimpleVector(2 * numPoints);
		
		for(int i = 0; i < 2 * numPoints; i++)
		{
			if(i < numPoints)
			{
				// TODO
			}
			
			if(i >= numPoints)
			{
				// TODO
			}
		}
		
		// Calculate Pseudo Inverse of m
		SimpleMatrix m_inv;
		m_inv = m.inverse(InversionType.INVERT_SVD);
		
		// Calculate Parameters with the help of the Pseudo Inverse
		SimpleVector x;
		x = SimpleOperators.multiply(m_inv, b);
		
		double r1 = x.getElement(0);
		double r2 = x.getElement(1);
		double t1 = x.getElement(2);
		double t2 = x.getElement(3);
		
		
		// TODO: normalize r
		
		
		double phi = 0; // TODO
		
		// Write the result for the translation and the rotation into the result vector
		SimpleVector result = new SimpleVector(phi, t1, t2);
		
		return result;
	}
	
	
	private SimpleMatrix applyTransform(SimpleMatrix points, double phi, SimpleVector translation){
		
		SimpleMatrix r = new SimpleMatrix(2,2);
		// TODO: fill the rotation matrix
		
		// TODO
		// TODO
		// TODO
		// TODO

		SimpleMatrix transformedPoints = new SimpleMatrix(points.getRows(), points.getCols());
				
		for(int i = 0; i < transformedPoints.getRows(); i++)
		{
			// TODO: transform points
			
		}
		
		return transformedPoints;
	}
	
	public static void main(String[] args){
		
		// Define Point Cloud 
		SimpleMatrix p_k = new SimpleMatrix(4,2);
		SimpleMatrix q_k = new SimpleMatrix(4,2);
		
		p_k.setRowValue(0, new SimpleVector(1,2));
		p_k.setRowValue(1, new SimpleVector(3,6));
		p_k.setRowValue(2, new SimpleVector(4,6));
		p_k.setRowValue(3, new SimpleVector(3,4));
		
		q_k.setRowValue(0, new SimpleVector(-0.7, 2.1));
		q_k.setRowValue(1, new SimpleVector(-2.1, 6.4));
		q_k.setRowValue(2, new SimpleVector(-1.4, 7.1));
		q_k.setRowValue(3, new SimpleVector(-0.7, 4.9));
		
		// Plot Point Cloud
		Plot plot = new Plot("Regression Line", "X", "Y", Plot.DEFAULT_FLAGS);
		plot.setLimits(-10, 10, -10, 10);
		plot.addPoints(p_k.getCol(0).copyAsDoubleArray(), p_k.getCol(1).copyAsDoubleArray(), Plot.BOX);
		plot.addPoints(q_k.getCol(0).copyAsDoubleArray(), q_k.getCol(1).copyAsDoubleArray(), Plot.CIRCLE);
		
		// Calculate registration parameter
		Registration1 reg = new Registration1();
		SimpleVector parameter = reg.registrationUsingPointCorrespondences(p_k, q_k);
		
		// Rotation and translation
		double phi = parameter.getElement(0);
		SimpleVector translation = new SimpleVector(parameter.getElement(1), parameter.getElement(2));
		
		// Transform points
		SimpleMatrix transformedPoints = reg.applyTransform(q_k, phi, translation);
		
		// Add transformed point cloud to the plot
		plot.addPoints(transformedPoints.getCol(0).copyAsDoubleArray(), transformedPoints.getCol(1).copyAsDoubleArray(), Plot.CROSS);
		plot.show();
		
	}
	
	
}