content: /*
   * Copyright (C) 2010-2014 Andreas Maier
   * CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
  */
  package edu.stanford.rsl.conrad.geometry.trajectories;
  
  
  import java.util.ArrayList;
  import java.util.Arrays;
  import java.util.Random;
  
  import edu.stanford.rsl.conrad.geometry.General;
  import edu.stanford.rsl.conrad.geometry.Projection;
  import edu.stanford.rsl.conrad.geometry.Rotations;
  import edu.stanford.rsl.conrad.geometry.Projection.CameraAxisDirection;
  import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
  import edu.stanford.rsl.conrad.geometry.shapes.simple.StraightLine;
  import edu.stanford.rsl.conrad.io.SafeSerializable;
  import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
  import edu.stanford.rsl.conrad.numerics.SimpleOperators;
  import edu.stanford.rsl.conrad.numerics.SimpleVector;
  import edu.stanford.rsl.conrad.numerics.Solvers;
  import edu.stanford.rsl.conrad.phantom.AnalyticPhantom;
  import edu.stanford.rsl.jpop.FunctionOptimizer;
  import edu.stanford.rsl.jpop.FunctionOptimizer.OptimizationMode;
  import edu.stanford.rsl.jpop.GradientOptimizableFunction;
  
  public class Trajectory implements SafeSerializable{
  
  	private static final long serialVersionUID = 8005225046964549262L;
  
  	protected int numProjectionMatrices;
  	protected int detectorWidth;
  	protected int detectorHeight;
  	protected double pixelDimensionX;
  	protected double pixelDimensionY;
  	protected double sourceToDetectorDistance;
  	protected double sourceToAxisDistance;
  	protected double averageAngularIncrement;
  	protected int projectionStackSize;
  	protected double [] reconDimensions;
  	protected double [] reconVoxelSizes;
  	protected Projection [] projectionMatrices;
  	protected double [] primaryAngles = null;
  	protected double [] secondaryAngles = null;
  	private String primaryAnglesString;
  	private String secondaryAnglesString;
  	protected double originInPixelsX;
  	protected double originInPixelsY;
  	protected double originInPixelsZ;
  	protected CameraAxisDirection detectorUDirection;
  	protected CameraAxisDirection detectorVDirection;
  	protected double detectorOffsetU;
  	protected double detectorOffsetV;
  	protected SimpleVector rotationAxis;
  
  	/**
  	 * Copy Constructor that reads the values of another Trajectory object.
  	 * @param source
  	 */
  	public Trajectory (Trajectory source){
  		this.numProjectionMatrices = source.numProjectionMatrices;
  		this.detectorWidth = source.detectorWidth;
  		this.detectorHeight = source.detectorHeight;
  		this.pixelDimensionX = source.pixelDimensionX;
  		this.pixelDimensionY = source.pixelDimensionY;
  		this.sourceToDetectorDistance = source.sourceToDetectorDistance;
  		this.sourceToAxisDistance = source.sourceToAxisDistance;
  		this.averageAngularIncrement = source.averageAngularIncrement;
  		if (source.reconDimensions != null) {
  			this.reconDimensions = Arrays.copyOf(source.reconDimensions, source.reconDimensions.length);
  		} else {
  			reconDimensions = new double[3];
  		}
  		if (source.reconVoxelSizes != null) {
  			this.reconVoxelSizes = Arrays.copyOf(source.reconVoxelSizes, source.reconVoxelSizes.length);
  		} else {
  			reconVoxelSizes = new double[3];
  		}
  		if (source.projectionMatrices != null){
  			this.projectionMatrices =  new Projection[source.projectionMatrices.length];
  			for(int  i = 0; i < source.projectionMatrices.length; i++)
  			{
  				this.projectionMatrices[i] = new Projection(source.projectionMatrices[i]);
  			}
  		}
  		if (source.primaryAngles != null){
  			this.primaryAngles =  Arrays.copyOf(source.primaryAngles, source.primaryAngles.length);
  		}
  		
  		if (source.secondaryAngles != null){
  			this.secondaryAngles =  Arrays.copyOf(source.secondaryAngles, source.secondaryAngles.length);
  		}
  		
  		this.projectionStackSize = source.projectionStackSize;
  		this.originInPixelsX = source.originInPixelsX;
  		this.originInPixelsY = source.originInPixelsY;
  		this.originInPixelsZ = source.originInPixelsZ;
  		this.detectorUDirection = source.detectorUDirection;
  		this.detectorVDirection = source.detectorVDirection;
  		if (source.rotationAxis != null) {
  			this.rotationAxis = source.rotationAxis.clone();
  		}
  	}
  
  	public Trajectory (){
  		reconDimensions = new double[3];
  		reconVoxelSizes = new double[3];
  	}
  
  
  	public Projection[] getProjectionMatrices() {
  		return projectionMatrices;
  	}
  
  	public void setProjectionMatrices(Projection[] projectionMatrices) {
  		this.projectionMatrices = projectionMatrices;
  	}
  
  	public int getDetectorWidth() {
  		return detectorWidth;
  	}
  
  	public void setDetectorWidth(int detectorWidth) {
  		this.detectorWidth = detectorWidth;
  	}
  
  	public int getDetectorHeight() {
  		return detectorHeight;
  	}
  
  	public void setDetectorHeight(int detectorHeight) {
  		this.detectorHeight = detectorHeight;
  	}
  
  	public double getPixelDimensionX() {
  		return pixelDimensionX;
  	}
  
  	public void setPixelDimensionX(double pixelDimensionX) {
  		this.pixelDimensionX = pixelDimensionX;
  	}
  
  	public double getPixelDimensionY() {
  		return pixelDimensionY;
  	}
  
  	public void setPixelDimensionY(double pixelDimensionY) {
  		this.pixelDimensionY = pixelDimensionY;
  	}
  
  	/**
  	 * returns the source to detector distance in [mm]
  	 * @return the source to detector distance
  	 */
  	public double getSourceToDetectorDistance() {
  		return sourceToDetectorDistance;
  	}
  
  	public void setSourceToDetectorDistance(double sourceToDetectorDistance) {
  		this.sourceToDetectorDistance = sourceToDetectorDistance;
  	}
  
  	public double getSourceToAxisDistance() {
  		return sourceToAxisDistance;
  	}
  
  	public void setSourceToAxisDistance(
  			double sourceToAxisDistance) {
  		this.sourceToAxisDistance = sourceToAxisDistance;
  	}
  
  	public double getAverageAngularIncrement() {
  		return averageAngularIncrement;
  	}
  
  	public void setAverageAngularIncrement(double averageAngularIncrement) {
  		this.averageAngularIncrement = averageAngularIncrement;
  	}
  
  	public double[] getReconDimensions() {
  		return reconDimensions;
  	}
  
  	public void setReconDimensions(double ... reconDimensions) {
  		this.reconDimensions = reconDimensions;
  	}
  
  	public void setNumProjectionMatrices(int numProjectionMatrices) {
  		this.numProjectionMatrices = numProjectionMatrices;
  	}
  
  	public double[] getReconVoxelSizes() {
  		return reconVoxelSizes;
  	}
  
  	public void setReconVoxelSizes(double[] reconVoxelSizes) {
  		this.reconVoxelSizes = reconVoxelSizes;
  	}
  
  	public Projection getProjectionMatrix(int i){
  		if (i < projectionMatrices.length && i>=0) 
  			return projectionMatrices[i];
  		else {
  			return null;
  		}
  	}
  
  	public void setProjectionMatrix(int i, Projection projMat){
  		if (i < projectionMatrices.length && i>=0) 
  		{
  			projectionMatrices[i] = projMat;
  		}
  	}
  	
  	public int getNumProjectionMatrices(){
  		return numProjectionMatrices;
  	}
  
  	public double[] getPrimaryAngles() {
  		return primaryAngles;
  	}
  
  	public void setPrimaryAngleArray(double[] primaryAngles) {
  		this.primaryAngles = primaryAngles;
  	}
  	
  	public double[] getSecondaryAngles(){
  		return secondaryAngles;
  	}
  	
  	public void setSecondaryAngleArray(double[] secondaryAngles) {
  		this.secondaryAngles = secondaryAngles;
  	}
  
  	public int getProjectionStackSize() {
  		return projectionStackSize;
  	}
  
  	public void setProjectionStackSize(int projectionStackSize) {
  		this.projectionStackSize = projectionStackSize;
  	}
  
  	public int getReconDimensionX() {
  		return (int)reconDimensions[0];
  	}
  
  	public int getReconDimensionY() {
  		return (int)reconDimensions[1];
  	}
  
  	public int getReconDimensionZ() {
  		return (int)reconDimensions[2];
  	}
  
  	public double getVoxelSpacingX() {
  		return reconVoxelSizes[0];
  	}
  
  	public double getVoxelSpacingY() {
  		return reconVoxelSizes[1];
  	}
  
  	public double getVoxelSpacingZ() {
  		return reconVoxelSizes[2];
  	}
  
  	public void setReconDimensionX(int value) {
  		reconDimensions[0] = value;
  	}
  
  	public void setReconDimensionY(int value) {
  		reconDimensions[1] = value;
  	}
  
  	public void setReconDimensionZ(int value) {
  		reconDimensions[2] = value;
  	}
  
  	public void setVoxelSpacingX(double value) {
  		reconVoxelSizes[0] = value;
  	}
  
  	public void setVoxelSpacingY(double value) {
  		reconVoxelSizes[1] = value;
  	}
  
  	public void setVoxelSpacingZ(double value) {
  		reconVoxelSizes[2] = value;
  	}
  
  	@Override
  	public void prepareForSerialization() {
  		if (primaryAngles != null) {
  			primaryAnglesString = "";
  			if (secondaryAngles != null) secondaryAnglesString = "";
  			for (int i=0; i< primaryAngles.length-1; i++){
  				primaryAnglesString += primaryAngles[i] + " ";
  				if (secondaryAngles != null)secondaryAnglesString += secondaryAngles[i] + " ";
  			}
  			primaryAnglesString += primaryAngles[primaryAngles.length-1];
  			if (secondaryAngles != null) secondaryAnglesString += secondaryAngles[secondaryAngles.length-1];
  			if (secondaryAngles != null) secondaryAngles = null;
  			primaryAngles = null;
  		}
  		if (projectionMatrices !=null) {
  			for (int i=0; i< projectionMatrices.length; i++){
  				//projectionMatrices[i].prepareForSerialization();
  			}
  		}
  	}
  
  	/**
  	 * @return the primaryAnglesString
  	 */
  	public String getPrimaryAnglesString() {
  		return primaryAnglesString;
  	}
  
  	/**
  	 * @param primaryAnglesString the primaryAnglesString to set
  	 */
  	public void setPrimaryAnglesString(String primaryAnglesString) {
  		String [] entries = primaryAnglesString.split("\\s+");
  		primaryAngles = new double [entries.length];
  		for (int i = 0; i<entries.length; i++){
  			primaryAngles[i] = Double.parseDouble(entries[i]);
  		}
  		this.primaryAnglesString = primaryAnglesString;
  	}
  
  	/**
  	 * @return the secondaryAnglesString
  	 */
  	public String getSecondaryAnglesString() {
  		return secondaryAnglesString;
  	}
  
  	/**
  	 * @param secondaryAnglesString the secondaryAnglesString to set
  	 */
  	public void setSecondaryAnglesString(String secondaryAnglesString) {
  		String [] entries = secondaryAnglesString.split("\\s+");
  		secondaryAngles = new double [entries.length];
  		for (int i = 0; i<entries.length; i++){
  			secondaryAngles[i] = Double.parseDouble(entries[i]);
  		}
  		this.secondaryAnglesString = secondaryAnglesString;
  	}
  
  	/**
  	 * @return the originInPixelsX
  	 */
  	public double getOriginInPixelsX() {
  		return originInPixelsX;
  	}
  
  	// 
  
  	/**
  	 * @return the originX in world coordinates [mm]
  	 */
  	public double getOriginX() {
  		return General.voxelToWorld(-originInPixelsX, getVoxelSpacingX(), 0);
  	}
  
  	/**
  	 * @return the originY in world coordinates [mm]
  	 */
  	public double getOriginY() {
  		return General.voxelToWorld(-originInPixelsY, getVoxelSpacingY(), 0);
  	}
  
  	/**
  	 * @return the originZ in world coordinates [mm]
  	 */
  	public double getOriginZ() {
  		return General.voxelToWorld(-originInPixelsZ, getVoxelSpacingZ(), 0);
  	}
  	
  	/**
  	 * Method to center the world origin around a phantom's min and max.
  	 * @param phantom
  	 */
  	public void setOriginToPhantomCenter(AnalyticPhantom phantom){
  		PointND min = phantom.getMin().clone();
  		PointND max = phantom.getMax().clone();
  		SimpleVector minVec = min.getAbstractVector();
  		minVec.add(max.getAbstractVector());
  		minVec.divideBy(2);
  		SimpleVector maxVec = new SimpleVector(getVoxelSpacingX()*getReconDimensionX(),getVoxelSpacingY()*getReconDimensionY(), getVoxelSpacingZ()*getReconDimensionZ());
  		maxVec.divideBy(2);
  		minVec.subtract(maxVec);
  		maxVec.multipliedBy(2);
  		maxVec.add(minVec);
  		setOriginInWorld(new PointND(minVec));	
  	}
  
  	public void setOriginInWorld(PointND originInWorld){
  		originInPixelsX = General.worldToVoxel(-originInWorld.get(0), getVoxelSpacingX(), 0);
  		originInPixelsY = General.worldToVoxel(-originInWorld.get(1), getVoxelSpacingY(), 0);
  		originInPixelsZ = General.worldToVoxel(-originInWorld.get(2), getVoxelSpacingZ(), 0);
  	}
  	
  	
  	/**
  	 * @param originInPixelsX the originInPixelsX to set
  	 */
  	public void setOriginInPixelsX(double originInPixelsX) {
  		this.originInPixelsX = originInPixelsX;
  	}
  
  	/**
  	 * @return the originInPixelsY
  	 */
  	public double getOriginInPixelsY() {
  		return originInPixelsY;
  	}
  
  	/**
  	 * @param originInPixelsY the originInPixelsY to set
  	 */
  	public void setOriginInPixelsY(double originInPixelsY) {
  		this.originInPixelsY = originInPixelsY;
  	}
  
  	/**
  	 * @return the originInPixelsZ
  	 */
  	public double getOriginInPixelsZ() {
  		return originInPixelsZ;
  	}
  
  	/**
  	 * @param originInPixelsZ the originInPixelsZ to set
  	 */
  	public void setOriginInPixelsZ(double originInPixelsZ) {
  		this.originInPixelsZ = originInPixelsZ;
  	}
  
  	/**
  	 * @param detectorUDirection the detectorUDirection to set
  	 */
  	public void setDetectorUDirection(CameraAxisDirection detectorUDirection) {
  		this.detectorUDirection = detectorUDirection;
  	}
  
  	/**
  	 * @return the detectorUDirection
  	 */
  	public CameraAxisDirection getDetectorUDirection() {
  		return detectorUDirection;
  	}
  
  	/**
  	 * @return the detectorVDirection
  	 */
  	public CameraAxisDirection getDetectorVDirection() {
  		return detectorVDirection;
  	}
  
  	/**
  	 * @param detectorVDirection the detectorVDirection to set
  	 */
  	public void setDetectorVDirection(CameraAxisDirection detectorVDirection) {
  		this.detectorVDirection = detectorVDirection;
  	}
  
  	/**
  	 * @return the detectorOffsetU
  	 */
  	public double getDetectorOffsetU() {
  		return detectorOffsetU;
  	}
  
  	/**
  	 * @param detectorOffsetU the detectorOffsetU to set
  	 */
  	public void setDetectorOffsetU(double detectorOffsetU) {
  		this.detectorOffsetU = detectorOffsetU;
  	}
  
  	/**
  	 * @return the detectorOffsetV
  	 */
  	public double getDetectorOffsetV() {
  		return detectorOffsetV;
  	}
  
  	/**
  	 * @param detectorOffsetV the detectorOffsetV to set
  	 */
  	public void setDetectorOffsetV(double detectorOffsetV) {
  		this.detectorOffsetV = detectorOffsetV;
  	}
  
  	/**
  	 * @param rotationAxis the rotationAxis to set
  	 */
  	public void setRotationAxis(SimpleVector rotationAxis) {
  		this.rotationAxis = rotationAxis;
  	}
  
  	/**
  	 * @return the rotationAxis
  	 */
  	public SimpleVector getRotationAxis() {
  		return rotationAxis;
  	}
  
  	/**
  	 * computes the iso center of the current trajectory. Current computation is based on heuristics, but works.
  	 * Should be implemented properly in the future.
  	 * Testing with 100 tries gives a standard deviation of 0.15 mm per coordinate.<br>
  	 * Average error is about 0.5 mm.
  	 * Use computeIsoCenter() instead!
  	 * 
  	 * @return the iso center of the scan trajectory.
  	 * @see #computeIsoCenter()
  	 */
  	public PointND computeIsoCenterOld(){
  		Random random = new Random(System.currentTimeMillis());
  		ArrayList<PointND> list = new ArrayList<PointND>();
  		for (int i = 0; i < 150; i++){
  			int random1 = random.nextInt(getProjectionMatrices().length);
  			Projection proj1 = getProjectionMatrices()[random1];
  			StraightLine line1 = new StraightLine(new PointND(proj1.computeCameraCenter()),proj1.computePrincipalAxis().normalizedL2());
  			int random2 = random.nextInt(getProjectionMatrices().length);
  			Projection proj2 = getProjectionMatrices()[random2];
  			StraightLine line2 = new StraightLine(new PointND(proj2.computeCameraCenter()),proj2.computePrincipalAxis().normalizedL2());
  			try {
  				PointND p = line1.intersect(line2, 2000);
  				if (p!= null){
  					list.add(p);
  				}
  			} catch(Exception e){
  
  			}
  		}
  		PointND center = new PointND(0,0,0);
  		for (PointND p:list){
  			center.getAbstractVector().add(p.getAbstractVector());
  		}
  		center.getAbstractVector().divideBy(list.size());
  		return center;
  	}
  	
  	/**
  	 * Call of this Method will replace the entries in the internal primaryAngle Array with values that are computed
  	 * from the projection matrices.
  	 * Do not call this too often, the gradient descent is quite slow.
  	 */
  	public void updatePrimaryAngles(){
  		// Internal Coordinates we want to use for visualization: (0,0,0) to (1,1,1);
  		
  		Trajectory trajectory = this;
  		if(primaryAngles == null){
  			primaryAngles = new double[trajectory.getProjectionMatrices().length];
  		}
  
  		PointND min = new PointND(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
  		PointND max = new PointND(-Double.MAX_VALUE, -Double.MAX_VALUE, -Double.MAX_VALUE);
  		for (Projection p: trajectory.getProjectionMatrices()){
  			PointND source = new PointND(p.computeCameraCenter());
  			min.updateIfLower(source);
  			max.updateIfHigher(source);
  		}
  		PointND center = trajectory.computeIsoCenterSolve();
  		SimpleVector range = max.getAbstractVector();
  		range.add(min.getAbstractVector().negated());		
  		
  		for (int i=0;i<3;i++){
  			if (range.getElement(i) < 0.1) range.setElementValue(i, 0.1);
  		}
  		// direction in the first halfspace
  		SimpleVector normdir = new SimpleVector(1,0,0);
  		// direction for the angular computation in the other halfspace
  		SimpleVector normdir2 = new SimpleVector(-1,0,0);
  		// compute normalized source position.
  		SimpleVector scaledCoordinate = trajectory.getProjectionMatrix(0).computeCameraCenter();
  		scaledCoordinate.add(center.getAbstractVector().negated());
  		scaledCoordinate.normalizeL2();
  		SimpleMatrix normRot = Rotations.getRotationMatrixFromAtoB(scaledCoordinate, normdir);
  		for (int i = 0; i < trajectory.getProjectionMatrices().length; i++){
  			// Normalize points and move them to the center
  			Projection p = trajectory.getProjectionMatrices()[i];
  			scaledCoordinate = p.computeCameraCenter();
  			scaledCoordinate.add(center.getAbstractVector().negated());
  			scaledCoordinate.normalizeL2();
  			scaledCoordinate = SimpleOperators.multiply(normRot, scaledCoordinate);
  			double angle = Rotations.getRotationFromAtoB(normdir, scaledCoordinate);
  			if (scaledCoordinate.getElement(1) > 0){
  				// Other half space:
  				angle =  Math.PI + Rotations.getRotationFromAtoB(normdir2, scaledCoordinate);
  			}
  			// primary Angles are stored in Degrees.
  			primaryAngles[i] = General.toDegrees(angle);
  		}
  		// set first angle to 0;
  		boolean invert = primaryAngles[0] > primaryAngles[1];
  		double substract = primaryAngles[0];
  		if (invert) substract = primaryAngles[primaryAngles.length-1];
  		
  		for (int i=primaryAngles.length-1; i >= 0;i--){
  			primaryAngles[i] -= substract;
  			primaryAngles[i] = primaryAngles[i] % 360;
  			if (primaryAngles[i] < 0) 
  				primaryAngles[i] = 360 + primaryAngles[i];
  		}
  		
  		
  	}
  
  	/**
  	 * Computes the iso center of the current trajectory.
  	 * Computation is based on a gradient descent procedure:<br>
  	 * The objective function is<br>
  	 * f(p, r) = (\Sum (x-p)^\top(x-p)-r^2)^2<br>
  	 * where<br>
  	 * p is the iso center and r the source to iso center distance. x are the different source positions.
  	 * The objective function is basically the sphere equation and we seek to find the closest point that fulfills the equation at 0.
  	 * Hence, we compute the square and use a gradient descent-type of solver.<br><br>
  	 * The derivatives are:<br>
  	 * f(p, r) / dp = 2 * (\Sum (x-p)^\top(x-p)-r^2) * (-2\Sum(x-p))<BR>
  	 * f(p, r) / dr = 2 * (\Sum (x-p)^\top(x-p)-r^2) * (-2\Sum(r))
  	 * 
  	 * We do 15 attempts just to be on the save side.
  	 * Still quite brute force, but better than guessing.<br>
  	 * Testing with 100 tries gives a standard deviation of 2.208233228965341E-15 per coordinate<br>
  	 * Average error is about 2.4312505535109182E-14.
  	 * 
  	 * @return the iso center of the scan trajectory.
  	 */
  	public PointND computeIsoCenter(){
  		// create list with source positions
  		ArrayList<PointND> list = new ArrayList<PointND>();
  		for (Projection p: this.getProjectionMatrices()){
  			list.add(new PointND(p.computeCameraCenter()));
  		}
  		IsoCenterObjectiveFunction function = new IsoCenterObjectiveFunction(list);
  		FunctionOptimizer fo = new FunctionOptimizer();
  		fo.setDimension(4);
  		fo.setOptimizationMode(OptimizationMode.Function);
  		fo.setConsoleOutput(false);
  		PointND center = null;
  		double min = Double.MAX_VALUE;
  		for (int i = 0; i < 15; i++) {
  			PointND firstGuess = computeIsoCenterOld();
  			//firstGuess = new PointND(0,0,0);
  			fo.setInitialX(new double [] {firstGuess.get(0),firstGuess.get(1),firstGuess.get(2),sourceToAxisDistance});
  			double [] result = fo.optimizeFunction(function);
  			if (function.evaluate(result, 0) < min) {
  				center = new PointND(result[0],result[1],result[2]);
  				min = function.evaluate(result, 0);
  			}
  		}
  		//System.out.println(firstGuess + " " + center);
  		return center;
  	}
  
  	private class IsoCenterObjectiveFunction implements GradientOptimizableFunction {
  
  		ArrayList<PointND> sourcePositions;
  
  		public IsoCenterObjectiveFunction(ArrayList<PointND> list){
  			sourcePositions = list;
  		}
  
  		@Override
  		public void setNumberOfProcessingBlocks(int number) {
  			// not parallel
  		}
  
  		@Override
  		public int getNumberOfProcessingBlocks() {
  			return 1;
  		}
  
  		@Override
  		public double evaluate(double[] x, int block) {
  			double sum = 0;
  			double r2 = x[3]*x[3];
  			for (PointND p: sourcePositions){
  				SimpleVector diff = new SimpleVector(p.get(0)-x[0],p.get(1)-x[1],p.get(2)-x[2]);
  				sum += SimpleOperators.multiplyInnerProd(diff, diff) - r2;
  			}
  			System.out.println("function " + sum*sum + " " + new SimpleVector(x));
  			return (sum*sum);
  		}
  
  		@Override
  		public double[] gradient(double[] x, int block) {
  			double [] grad = new double [4];
  			double sum =0;
  			double r2 = x[3]*x[3];
  			System.out.println("x: " + new SimpleVector(x));
  			try{
  				for (PointND p: sourcePositions){
  					SimpleVector diff = new SimpleVector(p.get(0)-x[0],p.get(1)-x[1],p.get(2)-x[2]);
  					sum += (SimpleOperators.multiplyInnerProd(diff, diff) - r2);
  					grad[0] += diff.getElement(0) * -2.0;
  					grad[1] += diff.getElement(1) * -2.0;
  					grad[2] += diff.getElement(2) * -2.0;
  					grad[3] += -2.0 * x[3];
  				}
  				grad[0] *= sum * 2.0;
  				grad[1] *= sum * 2.0;
  				grad[2] *= sum * 2.0;
  				grad[3] *= sum * 2.0;
  			} catch (Exception e){
  				e.printStackTrace();
  			}
  			System.out.println("grad: " + new SimpleVector(grad));
  			return grad;
  		}
  
  	}
  
  	/**
  	 * Computes the isocenter by a least-squares problem. The isocenter is computed by the 
  	 * smallest squared Eucledian distance to all principle rays.
  	 * See [1] for more details.
  	 * 
  	 * [1] J.-H. Choi, A. Maier, A. Keil, S. Pal, E. J. McWalter, G. S. Beaupré, 
  	 * G. E. Gold, and R. Fahrig, “Fiducial marker-based correction for involuntary 
  	 * motion in weight-bearing C-arm CT scanning of knees. II. Experiment,” 
  	 * Med. Phys., vol. 41, no. 6, p. 061902, Jun. 2014.
  	 */
  	public PointND computeIsoCenterSolve(){
  		
  		SimpleMatrix M = new SimpleMatrix(2*getProjectionMatrices().length,3);
  		SimpleVector vals = new SimpleVector(2*getProjectionMatrices().length);
  		
  		int idx = 0;
  		for (int i = 0; i < getProjectionMatrices().length; i++) {
  			Projection proj = getProjectionMatrices()[i];
  			SimpleMatrix projMat = proj.computeP();
  			SimpleVector vec3 = projMat.getSubRow(2, 0, 3);
  			SimpleVector vec2 = projMat.getSubRow(1, 0, 3);
  			SimpleVector vec1 = projMat.getSubRow(0, 0, 3);
  			double ppU = proj.getPrincipalPoint().getElement(0);
  			double ppV = proj.getPrincipalPoint().getElement(1);
  			M.setRowValue(idx, SimpleOperators.subtract(vec3.multipliedBy(ppU),vec1));
  			vals.setElementValue(idx, -ppU*projMat.getElement(2, 3)+projMat.getElement(0, 3));
  			idx++;
  			M.setRowValue(idx, SimpleOperators.subtract(vec3.multipliedBy(ppV),vec2));
  			vals.setElementValue(idx, -ppV*projMat.getElement(2, 3)+projMat.getElement(1, 3));
  			idx++;
  		}
  		
  		SimpleVector center = Solvers.solveLinearLeastSquares(M, vals);
  		return new PointND(center);
  	}
  
  
  
  }
  