tested LMA converging

src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java

@@ -861,4 +861,21 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
 		}
 		return true;
 	}
+	public static double [][] combineAffine(
+			double [][] ref_affine,
+			double [][] other_affine){
+		Matrix m_ref =   new Matrix(ref_affine);
+		Matrix m_other = new Matrix(other_affine);
+		Matrix A1 =  m_ref.getMatrix(0,1,0,1);
+		Matrix A2 =m_other.getMatrix(0,1,0,1);
+		Matrix B1 =  m_ref.getMatrix(0,1,2,2);
+		Matrix B2 =m_other.getMatrix(0,1,2,2);
+		Matrix A = A2.times(A1);
+		Matrix B = A2.times(B1).plus(B2);
+		double [][] affine = {
+				{A.get(0,0),A.get(0,1),B.get(0,0)}, 
+				{A.get(1,0),A.get(1,1),B.get(1,0)}};
+		return affine;
+	}
+	
 }

src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java

@@ -533,75 +533,104 @@ public class OrthoMapsCollection  implements Serializable{
 		// uses fixed_size gpu image size
 //		TDCorrTile [] td_corr_tiles =
 		TpTask [][] tp_tasks = new TpTask [2][];
-		double [][][] vector_field = 
-				ComboMatch.rectilinearVectorField(//rectilinearCorrelate_TD( // scene0/scene1
-				clt_parameters, // final CLTParameters          clt_parameters,
-				gpu_pair_img,        // final float  [][]            fpixels, // to check for empty
-				gpu_width,      // final int                    img_width,
-				woi, // Rectangle                    woi, // if null, use full GPU window
-				affines_gpu,         // final double [][][]          affine,  // [2][2][3] affine coefficients to translate common to 2 images
-				tp_tasks, // TpTask [][]                  tp_tasks_o,
-				false,          // final boolean                batch_mode,
-				debugLevel);    // final int                    debugLevel);
-		// may use tl_rect_metric to remap to the original image
-		double [][] tile_centers = new double [vector_field[0].length][];
-		int tilesX = gpu_width/GPUTileProcessor.DTT_SIZE; 
-		for (TpTask task: tp_tasks[1]) {
-			int ti = task.getTileY() * tilesX + task.getTileX();
-			tile_centers[ti] = task.getDoubleCenterXY();
-		}
-		if (show_tile_centers){
-			double [][] dbg_img = new double [6][tile_centers.length];
-			String [] dbg_titles = {"cX","cY","px0","py0", "px1","py1"};
-			for (int i = 0; i< dbg_img.length;i++) Arrays.fill(dbg_img[i], Double.NaN);
-			for (int t = 0; t < tp_tasks[0].length; t++) {
-				TpTask task0 = tp_tasks[0][t];
-				TpTask task1 = tp_tasks[1][t];
-				int ti = task0.getTileY() * tilesX + task0.getTileX();
-				dbg_img[0][ti] = task0.getDoubleCenterXY()[0]; // same for task0, task1 
-				dbg_img[1][ti] = task1.getDoubleCenterXY()[1]; 
-				dbg_img[2][ti] = task0.getXY()[0][0];
-				dbg_img[3][ti] = task0.getXY()[0][1];
-				dbg_img[4][ti] = task1.getXY()[0][0];
-				dbg_img[5][ti] = task1.getXY()[0][1];
-			} // getXY()
-    		ShowDoubleFloatArrays.showArrays(
-    				dbg_img,
-    				tilesX,
-    				tile_centers.length/tilesX,
-    				true,
-    				"tile_centers",
-    				dbg_titles);
-		}
-		
+		int num_tries = 5;
+		double prev_rms = Double.NaN;
+		double rel_improve = 1E-3;
+		for (int ntry = 0; ntry < num_tries; ntry++) {
+			double [][][] vector_field = 
+					ComboMatch.rectilinearVectorField(//rectilinearCorrelate_TD( // scene0/scene1
+							clt_parameters, // final CLTParameters          clt_parameters,
+							gpu_pair_img,        // final float  [][]            fpixels, // to check for empty
+							gpu_width,      // final int                    img_width,
+							woi, // Rectangle                    woi, // if null, use full GPU window
+							affines_gpu,         // final double [][][]          affine,  // [2][2][3] affine coefficients to translate common to 2 images
+							tp_tasks, // TpTask [][]                  tp_tasks_o,
+							false,          // final boolean                batch_mode,
+							debugLevel);    // final int                    debugLevel);
+			// may use tl_rect_metric to remap to the original image
+			double [][] tile_centers = new double [vector_field[0].length][];
+			int tilesX = gpu_width/GPUTileProcessor.DTT_SIZE; 
+			for (TpTask task: tp_tasks[1]) {
+				int ti = task.getTileY() * tilesX + task.getTileX();
+				tile_centers[ti] = task.getDoubleCenterXY();
+			}
+			if (show_tile_centers){
+				double [][] dbg_img = new double [6][tile_centers.length];
+				String [] dbg_titles = {"cX","cY","px0","py0", "px1","py1"};
+				for (int i = 0; i< dbg_img.length;i++) Arrays.fill(dbg_img[i], Double.NaN);
+				for (int t = 0; t < tp_tasks[0].length; t++) {
+					TpTask task0 = tp_tasks[0][t];
+					TpTask task1 = tp_tasks[1][t];
+					int ti = task0.getTileY() * tilesX + task0.getTileX();
+					dbg_img[0][ti] = task0.getDoubleCenterXY()[0]; // same for task0, task1 
+					dbg_img[1][ti] = task1.getDoubleCenterXY()[1]; 
+					dbg_img[2][ti] = task0.getXY()[0][0];
+					dbg_img[3][ti] = task0.getXY()[0][1];
+					dbg_img[4][ti] = task1.getXY()[0][0];
+					dbg_img[5][ti] = task1.getXY()[0][1];
+				} // getXY()
+				ShowDoubleFloatArrays.showArrays(
+						dbg_img,
+						tilesX,
+						tile_centers.length/tilesX,
+						true,
+						"tile_centers",
+						dbg_titles);
+			}
+
+
+			OrthoPairLMA orthoPairLMA = new OrthoPairLMA();
+			// vector_field[1] - neighbors
+			double lambda =            0.1;
+			double lambda_scale_good = 0.5;
+			double lambda_scale_bad =  8.0;
+			double lambda_max =      100;
+			double rms_diff =          0.001;
+			int    num_iter =         20;
+			boolean last_run =       false;
+			orthoPairLMA.prepareLMA(
+					// will always calculate relative affine, starting with unity
+					tilesX, // int           width,
+					vector_field[1], // double [][]   vector_XYS,    // optical flow X,Y, confidence obtained from the correlate2DIterate()
+					tile_centers, // double [][]   centers,       // tile centers (in pixels)
+					null, // double []     weights_extra, // optional, may be null 
+					true, // boolean       first_run,
+					debugLevel); // final int     debug_level)
+			int lma_rslt = orthoPairLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+					lambda,             // double lambda,           // 0.1
+					lambda_scale_good,  // double lambda_scale_good,// 0.5
+					lambda_scale_bad,   // double lambda_scale_bad, // 8.0
+					lambda_max,         // double lambda_max,       // 100
+					rms_diff,           // double rms_diff,         // 0.001
+					num_iter,           // int    num_iter,         // 20
+					last_run,           // boolean last_run,
+					debugLevel);       // int    debug_level)
+			if (debugLevel > -1) {
+				System.out.println("LMA result = "+lma_rslt);
+			}
+			if (lma_rslt < 0) {
+				System.out.println("LMA failed, result="+lma_rslt);
+				return null;
+			}
+			double rms = orthoPairLMA.getRms();
+			if (rms > prev_rms) {
+				if (debugLevel > -3) {
+				System.out.println("LMA RMSE worsened: new"+rms+" ("+ orthoPairLMA.getInitialRms()+"), prev="+prev_rms);
+				}
+				break;
+			}
+			affines_gpu[1]=OrthoMap.combineAffine(affines_gpu[1], orthoPairLMA.getAffine());
+			double [][] jtj = orthoPairLMA.getLastJtJ();
+			if ((prev_rms - rms)/prev_rms < rel_improve) {
+				if (debugLevel > -2) {
+					System.out.println("LMA relative RMSE improvement = "+((prev_rms - rms)/prev_rms)+" < "+rel_improve+", exiting.");
+				}
+				break;
+			}
+			prev_rms=rms;
+		} // for (int ntry = 0; ntry < num_tries; ntry++) {
+
 		
-		OrthoPairLMA orthoPairLMA = new OrthoPairLMA();
-		// vector_field[1] - neighbors
-		double lambda =            0.1;
-		double lambda_scale_good = 0.5;
-		double lambda_scale_bad =  8.0;
-		double lambda_max =      100;
-		double rms_diff =          0.001;
-		int    num_iter =         20;
-		boolean last_run =       false;
-		orthoPairLMA.prepareLMA(
-				// will always calculate relative affine, starting with unity
-				tilesX, // int           width,
-				vector_field[1], // double [][]   vector_XYS,    // optical flow X,Y, confidence obtained from the correlate2DIterate()
-				tile_centers, // double [][]   centers,       // tile centers (in pixels)
-				null, // double []     weights_extra, // optional, may be null 
-				true, // boolean       first_run,
-				debugLevel); // final int     debug_level)
-		int lma_rslt = orthoPairLMA.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
-				lambda,             // double lambda,           // 0.1
-				lambda_scale_good,  // double lambda_scale_good,// 0.5
-				lambda_scale_bad,   // double lambda_scale_bad, // 8.0
-				lambda_max,         // double lambda_max,       // 100
-				rms_diff,           // double rms_diff,         // 0.001
-				num_iter,           // int    num_iter,         // 20
-				last_run,           // boolean last_run,
-				debugLevel);       // int    debug_level)
-		System.out.println("LMA result = "+lma_rslt);
 // analyze result, re-run correlation
 		
 /*
@@ -637,7 +666,7 @@ public class OrthoMapsCollection  implements Serializable{
 		}
 
  */
-		if (debugLevel > 1) {
+		if (debugLevel > 1) {// show result here
 			String [] map_names = {ortho_maps[gpu_pair[0]].getName(),ortho_maps[gpu_pair[1]].getName()};
 			ShowDoubleFloatArrays.showArrays( 
 					gpu_pair_img,

src/main/java/com/elphel/imagej/orthomosaic/OrthoPairLMA.java

@@ -99,6 +99,30 @@ public class OrthoPairLMA {
 		
 	}
 	
+	public double [][] getAffine(){
+		return new double [][] {
+			{parameters_vector[0],parameters_vector[1],parameters_vector[4]},
+			{parameters_vector[2],parameters_vector[3],parameters_vector[5]}};
+	}
+	public double getRms() {
+		return last_rms[0];
+		
+	}
+	public double getInitialRms() {
+		return initial_rms[0];
+	}
+	public double getWeight() {
+		return weight;
+	}
+	public double [][] getLastJtJ(){
+		return getWJtJlambda(
+				0.0, // lambda, // *10, // temporary
+				this.last_jt);
+	}
+	//getWJtJlambda(
+//	lambda, // *10, // temporary
+//	this.last_jt)
+	
 	private void setSamplesWeightsYCenters(
 			final double [][] vector_XYS,
 			final double []   weights_extra, // null or additional weights (such as elevation-based)