Added focus/tilt adjustment using LMA

src/main/java/Aberration_Calibration.java

@@ -46,6 +46,9 @@ import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 
 
+
+
+//import FocusingField.FocusingFieldMeasurement;
 //import FocusingField.MeasuredSample;
 import Jama.Matrix;  // Download here: http://math.nist.gov/javanumerics/jama/
 
@@ -799,7 +802,8 @@ if (MORE_BUTTONS) {
 		addButton("List qualB",     panelCurvature,color_report);
 		addButton("List curv",      panelCurvature,color_report);
 		addButton("Show curv corr", panelCurvature,color_report);
-		addButton("Test measurement", panelCurvature,color_process);
+		addButton("Test measurement", panelCurvature,color_debug);
+		addButton("Focus/Tilt LMA", panelCurvature,color_process);
 		add(panelCurvature);
 		
 	//panelGoniometer
@@ -4513,6 +4517,26 @@ if (MORE_BUTTONS) {
 			FOCUSING_FIELD.testMeasurement();
 			return;
 		}
+/* ======================================================================== */
+		if       (label.equals("Focus/Tilt LMA")) {
+			DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
+			if (FOCUSING_FIELD==null) return;
+			FOCUSING_FIELD.setDebugLevel(DEBUG_LEVEL);
+			if (!FOCUS_MEASUREMENT_PARAMETERS.cameraIsConfigured) {
+				if (CAMERAS.showDialog("Configure cameras interface", 1, true)){
+					FOCUS_MEASUREMENT_PARAMETERS.cameraIsConfigured=true;
+					if (!FOCUS_MEASUREMENT_PARAMETERS.getLensSerial()) return;
+					// reset histories
+					MOTORS.clearPreFocus();
+					MOTORS.clearHistory();
+				} else {
+					IJ.showMessage("Error","Camera is not configured\nProcess canceled");
+					return;
+				}
+			}
+			while (adjustFocusTiltLMA());
+			return;
+		}
 //		
 /* ======================================================================== */
 		if       (label.equals("Show PSF")) {
@@ -8661,8 +8685,121 @@ if (MORE_BUTTONS) {
 		return;
 	}
 	
-	
 /* ===== Other methods ==================================================== */
+	public boolean adjustFocusTiltLMA(){
+		// just for reporting distance old way
+		MOTORS.focusingHistory.optimalMotorPosition( // recalculate calibration to estimate current distance from center PSF
+				FOCUS_MEASUREMENT_PARAMETERS,
+    			MOTORS.getMicronsPerStep(), //double micronsPerStep,
+    			DEBUG_LEVEL);
+		// No-move measure, add to history
+		moveAndMaybeProbe(
+				true, // just move, not probe
+				null, // no move, just measure
+				MOTORS,
+				CAMERAS,
+				LENS_DISTORTION_PARAMETERS,
+				matchSimulatedPattern, // should not bee null - is null after grid center!!!
+				FOCUS_MEASUREMENT_PARAMETERS,
+				PATTERN_DETECT,
+				DISTORTION,
+				SIMUL,
+				COMPONENTS,
+				OTF_FILTER,
+				PSF_PARS,
+				THREADS_MAX,
+				UPDATE_STATUS,
+				MASTER_DEBUG_LEVEL,
+				DISTORTION.loop_debug_level);
+		//get measurement
+		FocusingField.FocusingFieldMeasurement fFMeasurement=MOTORS.getThisFFMeasurement(FOCUSING_FIELD);	
+		// calculate z, tx, ty, m1,m2,m3
+	    double [] zTxTyM1M2M3 = FOCUSING_FIELD.adjustLMA (fFMeasurement);
+		// show dialog: Apply, re-calculate, exit
+    	int [] currentMotors=fFMeasurement.motors;
+    	int [] newMotors=currentMotors.clone();
+    	double [] zTxTy={Double.NaN,Double.NaN,Double.NaN};
+    	if (zTxTyM1M2M3!=null){
+    		newMotors[0]=(int) Math.round(zTxTyM1M2M3[3]);
+    		newMotors[1]=(int) Math.round(zTxTyM1M2M3[4]);
+    		newMotors[2]=(int) Math.round(zTxTyM1M2M3[5]);
+    		zTxTy[0]=zTxTyM1M2M3[0];
+    		zTxTy[1]=zTxTyM1M2M3[1];
+    		zTxTy[2]=zTxTyM1M2M3[2];
+    	}
+    	double [] targetTilts={0.0,0.0};
+    	double scaleMovement=1.0; // calculate automatically - reduce when close
+    	GenericDialog gd = new GenericDialog("Adjusting focus/tilt");
+        gd.addNumericField("Target focus (relative to best composirte)",FOCUSING_FIELD.targetRelFocalShift,2,5,"um ("+IJ.d2s(zTxTy[0],3)+")");
+        gd.addNumericField("Target horizontal tilt (normally 0)",targetTilts[0],2,5,"um/mm ("+IJ.d2s(zTxTy[1],3)+")");
+        gd.addNumericField("Target vertical tilt (normally 0)",targetTilts[1],2,5,"um/mm ("+IJ.d2s(zTxTy[2],3)+")");
+		gd.addNumericField("Motor 1",newMotors[0],0,5,"steps ("+currentMotors[0]+")");
+		gd.addNumericField("Motor 2",newMotors[1],0,5,"steps ("+currentMotors[1]+")");
+		gd.addNumericField("Motor 3",newMotors[2],0,5,"steps ("+currentMotors[2]+")");
+		gd.addNumericField("Scale movement",scaleMovement,3,5,"x");
+		
+        gd.addCheckbox("Filter samples/channels by Z",FOCUSING_FIELD.filterZ); // should be false after manual movement
+        gd.addCheckbox("Filter by value (leave lower than maximal fwhm used in focal scan mode)",FOCUSING_FIELD.filterByScanValue);
+        gd.addNumericField("Filter by value (remove samples above scaled best FWHM for channel/location)",FOCUSING_FIELD.filterByValueScale,2,5,"x");
+
+        gd.addNumericField("Z min",FOCUSING_FIELD.zMin,2,5,"um");
+        gd.addNumericField("Z max",FOCUSING_FIELD.zMax,2,5,"um");
+        gd.addNumericField("Z step",FOCUSING_FIELD.zStep,2,5,"um");
+
+        gd.addNumericField("Tilt min",FOCUSING_FIELD.tMin,2,5,"um/mm");
+        gd.addNumericField("Tilt max",FOCUSING_FIELD.tMax,2,5,"um/mm");
+        gd.addNumericField("Tilt step",FOCUSING_FIELD.tStep,2,5,"um/mm");
+		
+		gd.addNumericField("Motor anti-hysteresis travel (last measured was "+IJ.d2s(FOCUS_MEASUREMENT_PARAMETERS.measuredHysteresis,0)+")", FOCUS_MEASUREMENT_PARAMETERS.motorHysteresis, 0,7,"motors steps");
+		gd.addNumericField("Debug Level:",                                MASTER_DEBUG_LEVEL, 0);
+		gd.enableYesNoCancel("Apply movement","Re-measure"); // default OK (on enter) - "Apply"
+    	WindowTools.addScrollBars(gd);
+    	gd.showDialog();
+		if (gd.wasCanceled()) return false;
+		FOCUSING_FIELD.targetRelFocalShift=gd.getNextNumber();
+		targetTilts[0]=                    gd.getNextNumber();
+		targetTilts[1]=                    gd.getNextNumber();
+		newMotors[0]=               (int)  gd.getNextNumber();
+		newMotors[1]=               (int)  gd.getNextNumber();
+		newMotors[2]=               (int)  gd.getNextNumber();
+		scaleMovement=                     gd.getNextNumber();
+		
+        FOCUSING_FIELD.filterZ=            gd.getNextBoolean();
+        FOCUSING_FIELD.filterByScanValue=  gd.getNextBoolean();
+        FOCUSING_FIELD.filterByValueScale= gd.getNextNumber();
+
+        FOCUSING_FIELD.zMin=               gd.getNextNumber();
+        FOCUSING_FIELD.zMax=               gd.getNextNumber();
+        FOCUSING_FIELD.zStep=              gd.getNextNumber();
+
+        FOCUSING_FIELD.tMin=               gd.getNextNumber();
+        FOCUSING_FIELD.tMax=               gd.getNextNumber();
+        FOCUSING_FIELD.tStep=              gd.getNextNumber();
+		FOCUS_MEASUREMENT_PARAMETERS.motorHysteresis= (int) gd.getNextNumber();
+		MASTER_DEBUG_LEVEL=(         int)  gd.getNextNumber();
+		DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
+		FOCUSING_FIELD.setDebugLevel(DEBUG_LEVEL);
+		
+//	Scale motor movement	
+		newMotors[0]=currentMotors[0]+((int) Math.round((newMotors[0]-currentMotors[0])*scaleMovement));
+		newMotors[1]=currentMotors[1]+((int) Math.round((newMotors[1]-currentMotors[1])*scaleMovement));
+		newMotors[2]=currentMotors[2]+((int) Math.round((newMotors[2]-currentMotors[2])*scaleMovement));
+		
+		if (gd.wasOKed()){
+			// Move, no measure
+			MOTORS.moveElphel10364Motors(
+					true, //boolean wait,
+					newMotors,
+					0.0, //double sleep,
+					true, //boolean showStatus,
+					"",   //String message,
+					false); //!noHysteresis);			
+		}
+		return true;
+	}
+	
+	
+	
 	public boolean checkSerialAndRestore(){
 		// wait for camera
 		CAMERAS.debugLevel=DEBUG_LEVEL;

src/main/java/CalibrationHardwareInterface.java

@@ -3084,9 +3084,56 @@ public class CalibrationHardwareInterface {
     			pY0,
     			sampleCoord);
 	}
+	public FocusingField.FocusingFieldMeasurement getThisFFMeasurement(FocusingField focusingField){
+		return getThisFFMeasurement(focusingField, -1);
+	}    
+	public FocusingField.FocusingFieldMeasurement getThisFFMeasurement(FocusingField focusingField, int index){
+		return focusingField. getFocusingFieldMeasurement(
+				historyGetTimestamp(index),   //focusingState.getTimestamp(),
+				historyGetTemperature(index), //focusingState.getTemperature(),
+				historyGetMotors(index),      //focusingState.motorsPos,
+				historyGetSamples(index)); //focusingState.getSamples());
+	}
+	private FocusingHistory.FocusingState getFocusingState(int index){
+		if (index<0) index+=historySize();
+		if ((index>=0) && (index<historySize())) return this.focusingHistory.history.get(index);
+		return null;
+	}
 	
+//	public void addCurrentHistoryToFocusingField(FocusingField focusingField){
+//		this.focusingHistory.addCurrentHistory(focusingField);
+//	}
 	public void addCurrentHistoryToFocusingField(FocusingField focusingField){
-		this.focusingHistory.addCurrentHistory(focusingField);
+		for (int i=0;i<historySize();i++){
+			addCurrentHistoryToFocusingField(focusingField,i);
+		}        	 
+//		this.focusingHistory.addCurrentHistory(focusingField);
+	}
+
+	public void addCurrentHistoryToFocusingField(
+			FocusingField focusingField,
+			int index){ // -1 - last (negative - from length)
+		focusingField.addSample(
+				historyGetTimestamp(index),   //focusingState.getTimestamp(),
+				historyGetTemperature(index), //focusingState.getTemperature(),
+				historyGetMotors(index),      //focusingState.motorsPos,
+				historyGetSamples(index)); //focusingState.getSamples());
+	}
+	
+	
+	public String historyGetTimestamp(int index){
+		return getFocusingState(index).getTimestamp();
+	}
+
+	public double historyGetTemperature(int index){
+		return getFocusingState(index).getTemperature();
+	}
+	public int [] historyGetMotors(int index){
+		return getFocusingState(index).getMotors();
+	}
+	
+	public double [][][][] historyGetSamples(int index){
+		return getFocusingState(index).getSamples();
 	}
 
 	public int historySize(){
@@ -5470,7 +5517,7 @@ if (debugLevel>=debugThreshold) System.out.println(i+" "+diff[0]+" "+diff[1]+" "
     				Double.NaN,
     				Double.NaN);
     	}
-    	
+ /*   	
     	public void addCurrentHistory(
     			FocusingField focusingField)
     	{
@@ -5485,6 +5532,7 @@ if (debugLevel>=debugThreshold) System.out.println(i+" "+diff[0]+" "+diff[1]+" "
 
     	}
     	
+*/
     	public void saveXML(
     			String path,
         		String serialNumber,

src/main/java/FocusingField.java

@@ -3949,6 +3949,19 @@ public boolean LevenbergMarquardt(
             return nodeList;
         }
     }
+    public FocusingFieldMeasurement getFocusingFieldMeasurement(
+            String timestamp,
+            double temperature,
+            int [] motors,
+            double [][][][] samples
+            ){
+    	return new FocusingFieldMeasurement(
+                timestamp,
+                temperature,
+                motors,
+                samples);
+    }
+
     public FocusingField(
         String serialNumber,
             String lensSerial, // if null - do not add average
@@ -4341,11 +4354,69 @@ public boolean LevenbergMarquardt(
 //    	fieldFitting.mechanicalFocusingModel.setZTxTy(0.0,0.0,0.0); // restore zeros to correctly find Z centers,
     	fieldFitting.mechanicalFocusingModel.setAdjustMode(false); // to correctly find Z centers,
     }
+    
+//,
+    public double [] adjustLMA (FocusingFieldMeasurement measurement){
+    	if (!testMeasurement(
+    			measurement,    				
+				zMin, //+best_qb_corr[0],
+		        zMax, //+best_qb_corr[0],
+		        zStep, 
+				tMin,
+		        tMax,
+		        tStep)) {
+			if (debugLevel>0) System.out.println("adjustLMA() failed");
+    		return null;
+    	}
+    	double [] result=new double [6];
+        double [] best_qb_corr= fieldFitting.getBestQualB(
+                k_red,
+                k_blue,
+                true);
+        double [] zTilts=getCenterZTxTy(measurement);
+        result[0]=zTilts[0]-best_qb_corr[0];
+        result[1]=zTilts[1];
+        result[2]=zTilts[2];
+		double [] dm= getAdjustedMotors(
+	            targetRelFocalShift+best_qb_corr[0],
+	            0.0, // targetTiltX, // for testing, normally should be 0 um/mm
+	            0.0, // targetTiltY,
+	            true); // motor steps
+		if ((dm!=null) && (debugLevel>1)){
+			System.out.println("Suggested motor positions: "+IJ.d2s(dm[0],0)+":"+IJ.d2s(dm[1],0)+":"+IJ.d2s(dm[2],0));
+		}
+		if (dm!=null) {
+			result[3]=dm[0];
+			result[4]=dm[1];
+			result[5]=dm[2];
+		} else {
+			result[3]=Double.NaN;
+			result[4]=Double.NaN;
+			result[5]=Double.NaN;
+		}
+    	return result;
+    }
+    
+    // add tx, ty?
+    public double[] getCenterZTxTy(FocusingFieldMeasurement measurement){
+    	double [] tilts= fieldFitting.mechanicalFocusingModel.getTilts(measurement.motors);
+    	double [] result = {
+    			fieldFitting.mechanicalFocusingModel.calc_ZdZ(
+    					measurement.motors,
+    					currentPX0, //fieldFitting.getCenterXY()[0],
+    					currentPY0, //fieldFitting.getCenterXY()[1],
+    					null),
+    					tilts[0],
+    					tilts[1]
+    	};
+    	return result;
+    }
+    
     public double [] getAdjustedMotors(
             double targetRelFocalShift,
             double targetTiltX, // for testing, normally should be 0 um/mm
-            double targetTiltY,
-            boolean motorSteps){ // for testing, normally should be 0 um/mm
+            double targetTiltY,  // for testing, normally should be 0 um/mm
+            boolean motorSteps){ 
     	double [] zM=fieldFitting.mechanicalFocusingModel.getZM(
     			currentPX0, //fieldFitting.getCenterXY()[0],
     			currentPY0, //fieldFitting.getCenterXY()[1],
@@ -6370,6 +6441,25 @@ public boolean LevenbergMarquardt(
     		}
     		return z;
     	}
+        public double [] getTilts(int [] motors){
+    		double kM1=    getValue(MECH_PAR.K0)+getValue(MECH_PAR.KD1)-getValue(MECH_PAR.KD3);
+    		double kM2=    getValue(MECH_PAR.K0)-getValue(MECH_PAR.KD1)-getValue(MECH_PAR.KD3);
+    		double kM3=    getValue(MECH_PAR.K0)+getValue(MECH_PAR.KD3);
+    		double p2pi= PERIOD/2/Math.PI;
+    		double m1=motors[0],m2=motors[1],m3=motors[2];
+    		double aM1=(m1 + getValue(MECH_PAR.sM1)*p2pi*Math.sin(m1/p2pi) + getValue(MECH_PAR.cM1)*p2pi*Math.cos(m1/p2pi));
+    		double aM2=(m2 + getValue(MECH_PAR.sM2)*p2pi*Math.sin(m2/p2pi) + getValue(MECH_PAR.cM2)*p2pi*Math.cos(m2/p2pi));
+    		double aM3=(m3 + getValue(MECH_PAR.sM3)*p2pi*Math.sin(m3/p2pi) + getValue(MECH_PAR.cM3)*p2pi*Math.cos(m3/p2pi));
+    		double zM1=kM1 * aM1;
+    		double zM2=kM2 * aM2;
+    		double zM3=kM3 * aM3;
+        	double [] result ={
+        			getValue(MECH_PAR.tx)+(2*zM3-zM1-zM2)/(4*getValue(MECH_PAR.Lx)),
+        			getValue(MECH_PAR.ty)-(zM2-zM1)/(2*getValue(MECH_PAR.Ly))};
+        	return result;
+        }
+
+    	
     	
     	/**
     	 * Calculate linearized mount (motor) displacement from motor position in steps
@@ -6522,9 +6612,9 @@ public boolean LevenbergMarquardt(
     				}
     		};
     		double [][] B={
-    			{targetZ-getValue(MECH_PAR.z0)},
-    			{targetTx-dx*getValue(MECH_PAR.tx)},
-    			{targetTy-dy*getValue(MECH_PAR.ty)}
+    			{targetZ-getValue(MECH_PAR.z0)}, // calc_ZdZ()?
+    			{targetTx-getValue(MECH_PAR.tx)},
+    			{targetTy-getValue(MECH_PAR.ty)}
     		};
     		Matrix MA=new Matrix(A);
     		Matrix MB=new Matrix(B);