Debugging OrthoMultiLMA

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

@@ -157,6 +157,7 @@ public class ComboMatch {
 //		boolean show_map_stats =      false;      
 		boolean show_combo =          false; // true;
 		boolean create_overlaps =     false;
+		boolean create_map =          false;
 //		boolean show_combo_mask =     false; // generate image mas (where it is defined)_
 //		boolean use_alt =             false;
 //		boolean show_centers =        true;
@@ -231,6 +232,7 @@ public class ComboMatch {
 //		gd.addCheckbox    ("Show statistics for ortho images", show_map_stats, "Generate and show statistics for ortho maps.");
 		gd.addCheckbox    ("Show combo maps/stats",            show_combo, "Generate/save combo maps and stats.");
 		gd.addCheckbox    ("Create overlap pairs",             create_overlaps, "Create scene pairs overlaps.");
+		gd.addCheckbox    ("Create map",                       create_map, "Create combined map from pairwise matches.");
 //		gd.addCheckbox    ("Show combo image mask",            show_combo_mask, "Display combo binary image.");
 //		gd.addCheckbox    ("Show altitude combo image", use_alt, "Load and process altitude maps.");
 		gd.addNumericField("Remove fraction of worst matches",   frac_remove,  3,7,"",	"When fitting scenes remove this fraction of worst match.");
@@ -286,6 +288,7 @@ public class ComboMatch {
 		
 		show_combo =                gd.getNextBoolean();
 		create_overlaps =           gd.getNextBoolean();
+		create_map =                gd.getNextBoolean();
 		frac_remove =               gd.getNextNumber();
 		metric_error=               gd.getNextNumber();
 		if (use_marked_image ) { // will only be used if found and asked
@@ -768,6 +771,13 @@ public class ComboMatch {
     		} // final int debugLevel);
     	}
 
+		if (create_map) {
+			OrthoMultiLMA.buildOrthoMap(
+					clt_parameters,            // CLTParameters       clt_parameters,
+					maps_collection, // OrthoMapsCollection maps_collection
+					orthoMapsCollection_path); // String orthoMapsCollection_path
+			return true;
+		}
     	
 		if (create_overlaps) {
 			boolean ok =maps_collection.getIntersectedPairs(

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

@@ -28,12 +28,15 @@ package com.elphel.imagej.orthomosaic;
 import java.awt.Point;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.concurrent.atomic.AtomicInteger;
 
 import com.elphel.imagej.cameras.CLTParameters;
+import com.elphel.imagej.tileprocessor.ImageDtt;
 
 import Jama.Matrix;
 
 public class OrthoMultiLMA {
+	final boolean move_only;
 	private double []         last_rms =        null; // {rms, rms_pure}, matching this.vector
 	private double []         good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
 	private double []         initial_rms =     null; // {rms, rms_pure}, first-calcualted rms
@@ -48,14 +51,20 @@ public class OrthoMultiLMA {
 	private double []         last_ymfx =       null;
 	private double [][]       last_jt =         null;
 	private PairwiseOrthoMatch [][] matches =   null;
-	private ArrayList<Point>  pairs =           null;
+	private int [][]          pairs =           null;
+	private int [][]          pairs_ni =        null; // non-intersecting pairs for multi-threaded processing
 	private int []            indices =         null;
+	private double [][]       offsets =         null; // scene offsets (rd)
 	private int               num_scenes =      0;
 	private int               num_pairs =       0;
+	public OrthoMultiLMA (boolean move_only) {
+		this.move_only = move_only;
+	}
 	public static boolean testMultiLMA(
 			CLTParameters       clt_parameters,
 			OrthoMapsCollection maps_collection,
 			int []              indices) {
+		boolean       move_only = false;
 		int scene0 = indices[0], scene1 = indices[1];
 		PairwiseOrthoMatch direct = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
 		double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
@@ -75,12 +84,14 @@ public class OrthoMultiLMA {
 		double [][][] derivs_delta = new double [12][][];
 		double delta = 1E-7;
 		getAffineAndDerivetives_delta(
+				move_only, //boolean       move_only,
 				delta,   // double        delta,
 				affine0, // double [][]   affine00,
 				affine1, // double [][]   affine10,
 				rd,      // double []     rd,
 				derivs_delta); // double [][][] derivs)
-		double [][] affine = 		getAffineAndDerivetives(
+		double [][] affine = 		getAffineAndDerivatives(
+				move_only, //boolean       move_only,
 				affine0, // double [][]   affine00,
 				affine1, // double [][]   affine10,
 				rd,      // double []     rd,
@@ -96,6 +107,58 @@ public class OrthoMultiLMA {
 		System.out.println();
 		return true;
 	}
+	
+	public static int buildOrthoMap(
+			CLTParameters       clt_parameters,
+			OrthoMapsCollection maps_collection,
+			String orthoMapsCollection_path
+			) {
+		int                 debugLevel = 3;
+		boolean             move_only = false;
+		double []           val_coord = null; // 1 - valid, 0 - invalid, minimize coordinates errors
+		double              position_pull = 0.1;
+        boolean             use_inv = false;
+        double              overlap_pow = 2.0; // match weight as overlap fraction to this power
+		int [] indices = maps_collection.getScenesSelection(
+				null, // boolean select_all,
+				" to build a map"); // String purpose)
+		OrthoMultiLMA oml = new OrthoMultiLMA(move_only);
+		double lambda =            0.1;
+		double lambda_scale_good = 0.5;
+		double lambda_scale_bad =  8.0;
+		double lambda_max =      100;
+		double rms_diff =          0.0001;
+		int    num_iter =         20;
+		boolean last_run =       false;
+		oml.prepareLMA (
+				clt_parameters,   // CLTParameters       clt_parameters,
+				maps_collection,  // OrthoMapsCollection maps_collection,
+				indices,          // int []              indices,
+				val_coord,        // double []           val_coord, // 1 - valid, 0 - invalid, minimize coordinates errors
+				position_pull,    // double              position_pull,
+				use_inv,          // boolean             use_inv,
+				overlap_pow,      // double              overlap_pow, // match weight as overlap fraction to this power
+				debugLevel);      // int                 debugLevel)
+		int lma_rslt = oml.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,
+				null, // dbg_lma_prefix,     // String dbg_prefix,
+				debugLevel);        // int    debug_level)
+		System.out.println("lma_rslt="+lma_rslt); 
+        double [] rms = oml.getRms();
+        double [] initial_rms = oml.getInitialRms();
+        if (debugLevel > 0) {
+        	System.out.println("LMA: full RMS="+rms[0]+" ("+initial_rms[0]+"), pure RMS="+rms[1]+" ("+initial_rms[1]+")");
+        }
+        return 0;
+	}
+	
+	
 	public int prepareLMA (
 			CLTParameters       clt_parameters,
 			OrthoMapsCollection maps_collection,
@@ -108,7 +171,8 @@ public class OrthoMultiLMA {
 		this.indices = indices;
 		num_scenes = indices.length;
 		matches = new PairwiseOrthoMatch[indices.length][indices.length];
-		pairs = new ArrayList<Point>();
+		ArrayList<Point> pairs_list = new ArrayList<Point>();
+//		ArrayList<Double> offset_list = new ArrayList<Double>(); 
 		for (int i = 0; i < num_scenes-1; i++) {
 			int scene0 = indices[i]; 
 			for (int j = i+1; j < num_scenes; j++){
@@ -128,61 +192,95 @@ public class OrthoMultiLMA {
 						inv_match = match.getInverse(rd);
 					}
 				}
+				if (match != null) {
 					matches[i][j] = match;
 					matches[j][i] = inv_match;
-				pairs.add(new Point(i,j)); // only once?
+					pairs_list.add(new Point(i,j)); // only once?
+				}
 			}
 		}
-		num_pairs = pairs.size();
+		num_pairs = pairs_list.size();
+		offsets = new double [num_pairs][2];
+		pairs =   new int[num_pairs][2];
+		for (int np = 0; np < num_pairs; np++) {
+			Point pair = pairs_list.get(np);
+			pairs[np] = new int[] {pair.x, pair.y};
+			double [] enuOffset = maps_collection.ortho_maps[indices[pair.x]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.y]]);
+			offsets[np] = new double[] {enuOffset[0], -enuOffset[1]}; // {right,down} of the image 
+		}
+		pairs_ni = createNonIntersectingPairs(pairs);
 		if (val_coord == null) { // maybe not needed - use weights
 			val_coord = new double [indices.length];
 			Arrays.fill(val_coord, 1.0);
 		}
 		
-		y_vector =          new double [6*num_pairs + 2 * num_scenes]; // last 2 * num_scenes will stay 0
-		weights =           new double [6*num_pairs + 2 * num_scenes];
-		parameters_vector = new double [6*num_scenes]; // maybe will need move-only mode?
+		int n62 = move_only ? 2 : 6;
+		int n13 = move_only ? 1 : 3;
+		
+		
+		y_vector =          new double [n62*num_pairs + 2 * num_scenes]; // last 2 * num_scenes will stay 0
+		weights =           new double [n62*num_pairs + 2 * num_scenes];
+		parameters_vector = new double [n62*num_scenes]; // maybe will need move-only mode?
 		for (int n = 0; n < num_scenes; n++) {
 			double [][] affine = maps_collection.ortho_maps[indices[n]].getAffine();
-			System.arraycopy(affine[0], 0, parameters_vector, 6*n, 3);
-			System.arraycopy(affine[1], 0, parameters_vector, 6*n + 3, 3);
-			y_vector[6*num_pairs+2*n + 0] = affine[0][2]; 
-			y_vector[6*num_pairs+2*n + 1] = affine[1][2]; 
+			System.arraycopy(affine[0], 0, parameters_vector, n62*n, n13);
+			System.arraycopy(affine[1], 0, parameters_vector, n62*n + n13, n13);
+			y_vector[n62*num_pairs+2*n + 0] = affine[0][2]; 
+			y_vector[n62*num_pairs+2*n + 1] = affine[1][2]; 
 		}
+		double sw=0;
 		for (int np = 0; np < num_pairs; np++) {
-			Point pair_ind = pairs.get(np);
-			PairwiseOrthoMatch match = matches[pair_ind.x][pair_ind.y];
+			PairwiseOrthoMatch match = matches[pairs[np][0]][pairs[np][1]];
 			double [][] affine_pair = match.getAffine();
-			System.arraycopy(affine_pair[0], 0, y_vector, 6*np, 3);
-			System.arraycopy(affine_pair[1], 0, y_vector, 6*np + 3, 3);
+			if (move_only) {
+				System.arraycopy(affine_pair[0], 2, y_vector, n62*np, 1);
+				System.arraycopy(affine_pair[1], 2, y_vector, n62*np + 1, 1);
+			} else {
+				System.arraycopy(affine_pair[0], 0, y_vector, n62*np, 3);
+				System.arraycopy(affine_pair[1], 0, y_vector, n62*np + 3, 3);
+			}
 			double overlap = match.overlap;
 			double [][] jtj = match.jtj;
-			double [] w = new double [6];
+			double [] w = new double [n62];
+			if (move_only) {
+				w[0] = jtj[4][4]; w[1] = jtj[5][5];
+			} else {			
 				w[0] = jtj[0][0];w[1]=jtj[1][1]; w[2] = jtj[4][4]; // a00, a01, b0
 				w[3] = jtj[2][2];w[4]=jtj[3][3]; w[5] = jtj[5][5]; // a10, a11, b1
+			}
 			double w_overlap = Math.pow(overlap, overlap_pow);
 			for (int i = 0; i < w.length; i++) {
 				w[i] = Math.sqrt(w[i]) * w_overlap;
+				sw+=w[i];
 			}
-			System.arraycopy(w, 0, weights, 6*np, 6);
+			System.arraycopy(w, 0, weights, n62*np, n62);
 		}
-		int wi = 6*num_pairs;
+		double swp = sw;
+		int wi = n62 * num_pairs;
 		for (int n = 0; n < num_scenes; n++) {
 			double w = val_coord[n]*position_pull;
 			weights[wi++] = w;
 			weights[wi++] = w;
+			sw+= 2*w;
 		}		
-		
+		pure_weight = swp/sw;
+		double s = 1.0/sw;
+		for (int i = 0; i <weights.length;i++) {
+			weights[i] *=s;
+		}
+		last_jt = new double [parameters_vector.length][];
 		return weights.length;
 	}
 
 	public static double [][] getAffineAndDerivetives_delta(
+			boolean       move_only,
 			double        delta,
 			double [][]   affine00,
 			double [][]   affine10,
 			double []     rd,
 			double [][][] derivs) {
-		double [][] affine = getAffineAndDerivetives(
+		double [][] affine = getAffineAndDerivatives(
+				move_only, //boolean       move_only,
 				affine00, // double [][]   affine0,
 				affine10, // double [][]   affine1,
 				rd,      // double []     rd,
@@ -195,13 +293,15 @@ public class OrthoMultiLMA {
 							{affines0[0][0].clone(), affines0[0][1].clone()},
 							{affines0[1][0].clone(), affines0[1][1].clone()}};
 					affines[naff][row][col]= affines0[naff][row][col] - 0.5*delta;
-					double [][] affine_minus = getAffineAndDerivetives(
+					double [][] affine_minus = getAffineAndDerivatives(
+							move_only, //boolean       move_only,
 							affines[0], // double [][]   affine0,
 							affines[1], // double [][]   affine1,
 							rd,      // double []     rd,
 							null); // double [][][] derivs)
 					affines[naff][row][col]= affines0[naff][row][col] + 0.5*delta;
-					double [][] affine_plus = getAffineAndDerivetives(
+					double [][] affine_plus = getAffineAndDerivatives(
+							move_only, //boolean       move_only,
 							affines[0], // double [][]   affine0,
 							affines[1], // double [][]   affine1,
 							rd,      // double []     rd,
@@ -219,23 +319,92 @@ public class OrthoMultiLMA {
 		return affine;
 	}
 
-	public static double [][] getAffineAndDerivetives(
+	public static double [][] getAffineAndDerivatives(
+			boolean       move_only,
 			double [][]   affine0,
 			double [][]   affine1,
 			double []     rd,
-			double [][][] derivs) { // null or double[12] - will return affine derivatives by each of affine0, affine1
-		Matrix A0 = new Matrix (
-				new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
-		Matrix B0 = new Matrix(new double [][] {{affine0[0][2]},{affine0[1][2]}});
-		Matrix A1 = new Matrix (
-				new double [][] {{affine1[0][0],affine1[0][1]},{affine1[1][0],affine1[1][1]}});
-		Matrix B1 = new Matrix(new double [][] {{affine1[0][2]},{affine1[1][2]}});
-		Matrix V =  new Matrix(new double [][] {{-rd[0]},{-rd[1]}});
+			double [][][] derivs3) { // null or double[12] - will return affine derivatives by each of affine0, affine1
+		double [] params = new double[move_only? 4 : 12];
+		if (move_only) {
+			params[0] = affine0[0][2];
+			params[1] = affine0[1][2];
+			params[2] = affine1[0][2];
+			params[3] = affine1[1][2];
+			
+		} else {
+			System.arraycopy(affine0[0], 0, params, 0, 3);
+			System.arraycopy(affine0[1], 0, params, 3, 3);
+			System.arraycopy(affine1[0], 0, params, 6, 3);
+			System.arraycopy(affine1[1], 0, params, 9, 3);
+		}
+		double [][] derivs = (derivs3 !=null)? (new double[move_only? 4 : 12][]) : null;
+		double [] l_affine= getAffineAndDerivatives(  // [2]/[6]
+				move_only, //boolean       move_only,
+				params,  // double []     params,
+				0,       // int           pindx0,
+				1,       // int           pindx1,
+				rd,      // double []     rd,
+				derivs); // double [][][] derivs)
+		double [][] affine;
+		if (move_only) {
+			affine = new double [][]{
+				{1,0,l_affine[0]},
+				{0,1,l_affine[1]}};
+				if (derivs3 !=null) {
+					for (int i = 0; i < 4; i++) {
+						derivs3[3*i+2] = new double [][] {
+							{0,0,derivs[i][0]},
+							{0,0,derivs[i][1]}};
+					}
+				}
+		} else {
+			affine = new double [][]{{l_affine[0],l_affine[1],l_affine[2]},{l_affine[3],l_affine[4],l_affine[5]}};
+			if (derivs3 !=null) {
+				for (int i = 0; i < derivs3.length; i++) {
+					derivs3[i] = new double [][] {
+						{derivs[i][0],derivs[i][1],derivs[i][2]},
+						{derivs[i][3],derivs[i][4],derivs[i][5]}};
+				}
+			}
+		}
+		return affine;
+	}
+	
+	public static double [] getAffineAndDerivatives(
+			boolean       move_only,
+			double []     params,
+			int           pindx0,
+			int           pindx1,
+			double []     rd,
+			double [][]   derivs) { // null or double[12] ([4] for move_only) - will return affine derivatives by each of affine0, affine1
+		int indx0, indx1;
+		double [][] aff0_a, aff0_b, aff1_a, aff1_b;
+		
+		if (move_only) {
+			indx0 = 2 * pindx0;
+			indx1 = 2 * pindx1;
+			aff0_a = new double [][] {{1, 0},{0, 1}};
+			aff0_b = new double [][] {{params[indx0+0]},{params[indx0+1]}};
+			aff1_a = new double [][] {{1, 0},{0, 1}};
+			aff1_b = new double [][] {{params[indx1+0]},{params[indx1+1]}};
+		} else {
+			indx0 = 6 * pindx0;
+			indx1 = 6 * pindx1;
+			aff0_a = new double [][] {{params[indx0+0],params[indx0+1]},{params[indx0+3],params[indx0+4]}};
+			aff0_b = new double [][] {{params[indx0+2]},{params[indx0+5]}};
+			aff1_a = new double [][] {{params[indx1+0],params[indx1+1]},{params[indx1+3],params[indx1+4]}};
+			aff1_b = new double [][] {{params[indx1+2]},{params[indx1+5]}};
+		}
+		Matrix A0 = new Matrix (aff0_a);
+		Matrix B0 = new Matrix (aff0_b);
+		Matrix A1 = new Matrix (aff1_a);
+		Matrix B1 = new Matrix (aff1_b);
+		Matrix V =  new Matrix (new double [][] {{-rd[0]},{-rd[1]}});
 		Matrix A =  A1.times(A0.inverse());
 		Matrix B =  A.times(V.minus(B0)).minus(A1.times(V)).plus(B1);
-		double [][] affine = new double[][] {
-			{A.get(0,0),A.get(0,1), B.get(0,0)},
-			{A.get(1,0),A.get(1,1), B.get(1,0)}};
+		double [] affine =  move_only? (new double[] {B.get(0,0), B.get(1,0)}) :   
+			(new double[] {A.get(0,0),A.get(0,1), B.get(0,0),	A.get(1,0),A.get(1,1), B.get(1,0)});
 		if (derivs != null) {
 			double [][] a = A.getArray();
 			double [][] a0 = A0.getArray();
@@ -272,7 +441,6 @@ public class OrthoMultiLMA {
 			double [][] da_da101 = {{-idet*a0[1][0], idet*a0[0][0]},{0,0}};
 			double [][] da_da110 = {{ 0,        0       },{ idet*a0[1][1],-idet*a0[0][1]}};   
 			double [][] da_da111 = {{ 0,        0       },{-idet*a0[1][0], idet*a0[0][0]}}; 
-			
 			double [][][][] da_da1 = {{da_da100, da_da101}, {da_da110, da_da111}};
 			double [][][][][] da_da = {da_da0, da_da1};
 			double [] db_da000 = {
@@ -302,28 +470,39 @@ public class OrthoMultiLMA {
 					da_da111[1][0]*vb0[0]+da_da111[1][1]*vb0[1] - v[1]};
 			double [][][] db_da1 = {{db_da100,db_da101},{db_da110,db_da111}};
 			double [][][][] db_da = {db_da0,db_da1};
-			
 			double [] db_db00 = {-a[0][0],-a[1][0]};
 			double [] db_db01 = {-a[0][1],-a[1][1]};
 			double [] db_db10 = {1,0};
 			double [] db_db11 = {0,1};
 			double [][][] db_db = {{db_db00,db_db01},{db_db10,db_db11}};
+			if (move_only) {
+				for (int naff = 0; naff < da_da.length; naff++) {
+					for (int row = 0; row < da_da[naff].length; row++) {
+						int indx = naff*2+row;
+						derivs[indx] = new double[2]; // [2][3];
+						for (int i = 0; i < 2; i++) {
+							derivs[indx][i] = db_db[naff][row][i];
+						}							
+					}
+				}
+			} else {
 				// fill in  derivatives, skipping B - only 2x2 A
 				for (int naff = 0; naff < da_da.length; naff++) {
 					for (int row = 0; row < da_da[naff].length; row++) {
 						for (int col = 0; col <  3; col++) {
 							int indx = naff*6+row*3+col;
-						derivs[indx] = new double[2][3];
+							derivs[indx] = new double[6]; // [2][3];
 							if ((col <  da_da[naff][row].length) ) {
 								for (int i = 0; i < 2; i++) {
 									for (int j = 0; j< 2; j++) {
-									derivs[indx][i][j] = da_da[naff][row][col][i][j];
+										derivs[indx][3 * i + j] = da_da[naff][row][col][i][j];
 									}
-								derivs[indx][i][2] = db_da[naff][row][col][i];
+									derivs[indx][3 * i + 2] = db_da[naff][row][col][i];
 								}
 							} else { // d/db
 								for (int i = 0; i < 2; i++) {
-								derivs[indx][i][2] = db_db[naff][row][i];
+									derivs[indx][3 * i + 2] = db_db[naff][row][i];
+								}							
 							}
 						}
 					}
@@ -333,5 +512,451 @@ public class OrthoMultiLMA {
 		return affine; // _alt;
 	}
 	
+	private static int [][] createNonIntersectingPairs(int [][] pairs) {
+		ArrayList<Integer> pairs_list = new ArrayList<Integer>();
+		for (int i = 0; i < pairs.length; i++) {
+			pairs_list.add(i);
+		}
+		int max_scene = 0;
+		for (int [] pair:pairs) {
+			max_scene=Math.max(max_scene, pair[0]);
+			max_scene=Math.max(max_scene, pair[1]);
+		}
+		ArrayList<ArrayList<Integer>> list_list_pairs = new ArrayList<ArrayList<Integer>>();
+		while (!pairs_list.isEmpty()) {
+			boolean [] used = new boolean [max_scene+1];
+			ArrayList<Integer> pairs_new = new ArrayList<Integer>();
+			for (int i = 0; i < pairs_list.size(); ) { // no increment here!
+				int ip = pairs_list.get(i);
+				int [] p = pairs[ip];
+				if (used[p[0]] || used[p[1]]) {
+					i++;
+				} else {	
+					used[p[0]] = true;
+					used[p[1]] = true;
+					pairs_new.add(ip);
+					pairs_list.remove(i);
+				}
+			}
+			list_list_pairs.add(pairs_new);		
+		}
+		int [][] pairs_ni = new int [list_list_pairs.size()][];
+		for (int n = 0; n < pairs_ni.length; n++) {
+			pairs_list = list_list_pairs.get(n);
+			pairs_ni[n] = new int [pairs_list.size()];
+			for (int i = 0;i < pairs_ni[n].length; i++) {
+				pairs_ni[n][i] = pairs_list.get(i);
+			}
+		}
+		return pairs_ni;
+ 	}
+	
+	
+	
+
+	public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
+			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,
+			String dbg_prefix,
+			int    debug_level)
+	{
+		boolean [] rslt = {false,false};
+		this.last_rms = null; // remove?
+		int iter = 0;
+		if (dbg_prefix != null) {
+//			 debugStateImage(dbg_prefix+"-initial");
+		}
+		for (iter = 0; iter < num_iter; iter++) {
+			rslt =  lmaStep(
+					lambda,
+					rms_diff,
+					debug_level);
+			if (dbg_prefix != null) {
+//				 debugStateImage(dbg_prefix+"-step_"+iter);
+			}
+			
+			if (rslt == null) {
+				return -1; // false; // need to check
+			}
+			if (debug_level > 1) {
+				System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
+						" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+			}
+			if (rslt[1]) {
+				break;
+			}
+			if (rslt[0]) { // good
+				lambda *= lambda_scale_good;
+			} else {
+				lambda *= lambda_scale_bad;
+				if (lambda > lambda_max) {
+					break; // not used in lwir
+				}
+			}
+		}
+		if (rslt[0]) { // better
+			if (iter >= num_iter) { // better, but num tries exceeded
+				if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
+			}
+
+		} else { // improved over initial ?
+			if (last_rms[0] < initial_rms[0]) { // NaN
+				rslt[0] = true;
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
+			} else {
+				if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
+			}
+		}
+		boolean show_intermediate = true;
+		if (show_intermediate && (debug_level > 0)) {
+			System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+		}
+		if (debug_level > 2){ 
+			System.out.println("iteration="+iter);
+		}
+		if (debug_level > 0) {
+			if ((debug_level > 1) ||  last_run) { // (iter == 1) || last_run) {
+				if (!show_intermediate) {
+					System.out.println("LMA: iter="+iter+",   full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
+				}
+			}
+		}
+		if ((debug_level > -2) && !rslt[0]) { // failed
+			if ((debug_level > 1) || (iter == 1) || last_run) {
+				System.out.println("LMA failed on iteration = "+iter);
+			}
+			System.out.println();
+		}
+
+		return rslt[0]? iter : -1;
+	}
+	
+	private boolean [] lmaStep(
+			double lambda,
+			double rms_diff,
+			int debug_level) {
+		boolean [] rslt = {false,false};
+		// maybe the following if() branch is not needed - already done in prepareLMA !
+		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
+			last_rms = new double[2];
+			if (debug_level > 1) {
+				System.out.println("lmaStep(): first step");
+			}
+			double [] fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					last_rms); // final double []   rms_fp // null or [2]
+			this.initial_rms = this.last_rms.clone();
+			this.good_or_bad_rms = this.last_rms.clone();
+
+			if (debug_level > -1) { // temporary
+				/*
+				dbgYminusFxWeight(
+						this.last_ymfx,
+						this.weights,
+						"Initial_y-fX_after_moving_objects");
+                */
+			}
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			// TODO: Restore/implement
+			if (debug_level > 3) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+				*/
+			}
+		}
+		Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
+
+		Matrix wjtjlambda = new Matrix(getWJtJlambda(
+				lambda, // *10, // temporary
+				this.last_jt)); // double [][] jt)
+		
+		if (debug_level>2) {
+			System.out.println("JtJ + lambda*diag(JtJ");
+			wjtjlambda.print(18, 6);
+		}
+		Matrix jtjl_inv = null;
+		try {
+			jtjl_inv = wjtjlambda.inverse(); // check for errors
+		} catch (RuntimeException e) {
+			rslt[1] = true;
+			if (debug_level > 0) {
+				System.out.println("Singular Matrix!");
+			}
+
+			return rslt;
+		}
+		if (debug_level>2) {
+			System.out.println("(JtJ + lambda*diag(JtJ).inv()");
+			jtjl_inv.print(18, 6);
+		}
+//last_jt has NaNs
+		Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
+		if (debug_level>2) {
+			System.out.println("Jt * (y-fx)");
+			jty.print(18, 6);
+		}
+		
+		
+		Matrix mdelta = jtjl_inv.times(jty);
+		if (debug_level>2) {
+			System.out.println("mdelta");
+			mdelta.print(18, 6);
+		}
+
+		double scale = 1.0;
+		double []  delta =      mdelta.getColumnPackedCopy();
+		double []  new_vector = parameters_vector.clone();
+		for (int i = 0; i < parameters_vector.length; i++) {
+			new_vector[i] += scale * delta[i];
+		}
+		
+		double [] fx = getFxDerivs(
+				new_vector, // double []         vector,
+				last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+				debug_level);      // final int         debug_level)
+		double [] rms = new double[2];
+		last_ymfx = getYminusFxWeighted(
+//				vector_XYS, // final double [][] vector_XYS,
+				fx, // final double []   fx,
+				rms); // final double []   rms_fp // null or [2]
+		if (debug_level > 2) {
+			/*
+			dbgYminusFx(this.last_ymfx, "next y-fX");
+			dbgXY(new_vector, "XY-correction");
+			*/
+		}
+
+		if (last_ymfx == null) {
+			return null; // need to re-init/restart LMA
+		}
+
+		this.good_or_bad_rms = rms.clone();
+		if (rms[0] < this.last_rms[0]) { // improved
+			rslt[0] = true;
+			rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
+			this.last_rms = rms.clone();
+
+			this.parameters_vector = new_vector.clone();
+			if (debug_level > 2) {
+				// print vectors in some format
+				/*
+				System.out.print("delta: "+corr_delta.toString()+"\n");
+				System.out.print("New vector: "+new_vector.toString()+"\n");
+				System.out.println();
+				*/
+			}
+		} else { // worsened
+			rslt[0] = false;
+			rslt[1] = false; // do not know, caller will decide
+			// restore state
+			fx = getFxDerivs(
+					parameters_vector, // double []         vector,
+					last_jt,           // final double [][] jt, // should be null or initialized with [vector.length][]
+					debug_level);      // final int         debug_level)
+			last_ymfx = getYminusFxWeighted(
+					fx, // final double []   fx,
+					this.last_rms); // final double []   rms_fp // null or [2]
+			if (last_ymfx == null) {
+				return null; // need to re-init/restart LMA
+			}
+			if (debug_level > 2) {
+				/*
+				 dbgJacobians(
+							corr_vector, // GeometryCorrection.CorrVector corr_vector,
+							1E-5, // double delta,
+							true); //boolean graphic)
+							*/
+			}
+		}
+		return rslt;
+	}
+	
+
+	private double [][] getWJtJlambda(
+			final double      lambda,
+			final double [][] jt)
+	{
+		final int num_pars = jt.length;
+		final int num_pars2 = num_pars * num_pars;
+		final int nup_points = jt[0].length;
+		final double [][] wjtjl = new double [num_pars][num_pars];
+		final Thread[] threads = ImageDtt.newThreadArray();
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
+						int i = indx / num_pars;
+						int j = indx % num_pars;
+						if (j >= i) {
+							double d = 0.0;
+							for (int k = 0; k < nup_points; k++) {
+								if (jt[i][k] != 0) {
+									d+=0;
+								}
+								d += weights[k]*jt[i][k]*jt[j][k];
+							}
+							wjtjl[i][j] = d;
+							if (i == j) {
+								wjtjl[i][j] += d * lambda;
+							} else {
+								wjtjl[j][i] = d;
+							}
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return wjtjl;
+	}
+
+	private double [] getYminusFxWeighted(
+			final double []   fx,
+			final double []   rms_fp // null or [2]
+			) {
+		final int n62 = move_only ? 2 : 6; 
+		final Thread[]      threads =     ImageDtt.newThreadArray();
+		final AtomicInteger ai =          new AtomicInteger(0);
+		final double []     wymfw =       new double [fx.length];
+		double [] swd2 = new double[num_pairs];
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nPair = ai.getAndIncrement(); nPair < num_pairs; nPair = ai.getAndIncrement()) {
+						int np6 = n62*nPair;
+						for (int i1 = 0; i1 < n62; i1++) {
+							int i = np6+i1;
+							double d = y_vector[i] - fx[i];
+							double wd = d * weights[i];
+							wymfw[i] = wd;
+							swd2[nPair] += d * wd;
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		double s_rms = 0;
+		for (int nPair = 0; nPair < num_pairs; nPair++) {
+			s_rms += swd2[nPair];
+		}
+		double s_rms_pure = s_rms/pure_weight; 
+		// num_scenes is not that large; sum w/o threads
+		for (int nScene = 0; nScene < num_scenes; nScene++) {
+			for (int i1 = 0; i1 < 2; i1++) {
+				int i = n62*num_pairs + 2 * nScene + i1;
+				double d = y_vector[i] - fx[i];
+				double wd = d * weights[i];
+				wymfw[i] = wd;
+				s_rms +=  d * wd;
+			}
+		}
+		if (rms_fp != null) {
+			rms_fp[0] = Math.sqrt(s_rms);
+			if (rms_fp.length > 1) {
+				rms_fp[1] = Math.sqrt(s_rms_pure);
+			}
+		}
+		return wymfw;
+	}
+
+	public double [] getRms() {
+		return last_rms;
+		
+	}
+	public double [] getInitialRms() {
+		return initial_rms;
+	}	
+	
+	
+	private double [] getFxDerivs(
+			final double []   vector,
+			final double [][] jt, // should be null or initialized with [vector.length][]
+			final int         debug_level)
+	{
+		final int n62 = move_only ? 2 : 6;    
+		final double [] fx = new double [weights.length]; // weights.length]; : weights.length :
+		if (jt != null) {
+			for (int i = 0; i < jt.length; i++) {
+				jt[i] = new double [weights.length]; // weights.length];
+			}
+		}
+		final Thread[] threads = ImageDtt.newThreadArray();
+		final AtomicInteger ai = new AtomicInteger(0);
+		for (int pair_group = 0; pair_group<pairs_ni.length; pair_group++) {
+			final int fpair_group = pair_group;
+			for (int ithread = 0; ithread < threads.length; ithread++) {
+				threads[ithread] = new Thread() {
+					public void run() {
+						double [][] jt_frac = (jt != null)? (new double [2 * n62][]):null;
+						for (int iPair = ai.getAndIncrement(); iPair < pairs_ni[fpair_group].length; iPair = ai.getAndIncrement())  {
+							int nPair = pairs_ni[fpair_group][iPair]; // index in pairs[][]
+							int [] pair = pairs[nPair];
+							double [] fx_frac = getAffineAndDerivatives(
+									move_only,
+									vector,         // double []     params,
+									pair[0],        // int           pindx0,
+									pair[1],        // int           pindx1,
+									offsets[nPair], // double []     rd,
+									jt_frac);       // double [][]  derivs)
+							System.arraycopy(
+									fx_frac,
+									0,
+									fx,
+									n62 * nPair,
+									n62);
+							if (jt_frac != null) {
+								for (int ns = 0; ns < 2; ns++) {
+									for (int nsp = 0; nsp < n62; nsp++) {
+										for (int i = 0; i < n62; i++) {
+											jt[n62 * pairs[nPair][ns] + nsp][n62 * nPair + i] +=
+													jt_frac[n62 * ns +nsp][i];
+										}
+									}
+								}
+							}
+						}
+					}
+				};
+			}		      
+			ImageDtt.startAndJoin(threads);
+			ai.set(0);
+		}
+		final int pull_indx = n62 * num_pairs;
+		final int [] ioffs = {move_only? 0:2,move_only? 1:5};
+		for (int ithread = 0; ithread < threads.length; ithread++) {
+			threads[ithread] = new Thread() {
+				public void run() {
+					for (int nScene = ai.getAndIncrement(); nScene < num_scenes; nScene = ai.getAndIncrement()){
+						fx[pull_indx + 2 * nScene + 0] = vector[n62 * nScene + ioffs[0]];
+						fx[pull_indx + 2 * nScene + 1] = vector[n62 * nScene + ioffs[1]];
+						if (jt!= null) {
+							jt[n62 * nScene + ioffs[0]][pull_indx + 2 * nScene + 0] = 1.0;
+							jt[n62 * nScene + ioffs[1]][pull_indx + 2 * nScene + 1] = 1.0;
+						}
+					}
+				}
+			};
+		}		      
+		ImageDtt.startAndJoin(threads);
+		return fx;
+	}	
 
 }

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

@@ -720,7 +720,7 @@ public class OrthoPairLMA {
 	}
 	
 	
-	private double [][] getWJtJlambda( // USED in lwir
+	private double [][] getWJtJlambda(
 			final double      lambda,
 			final double [][] jt)
 	{
@@ -766,7 +766,6 @@ public class OrthoPairLMA {
 			final double [][] jt, // should be null or initialized with [vector.length][]
 			final int         debug_level)
 	{
-//		final double [][] affine = {{vector[0],vector[1],vector[4]},{vector[2],vector[3],vector[5]}};
 		final double [] fx = new double [weights.length]; // weights.length]; : weights.length :
 		if (jt != null) {
 			for (int i = 0; i < jt.length; i++) {

src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java

@@ -2583,7 +2583,7 @@ public class Corr2dLMA {
 			fx[i] = this.weights[i] * d;
 			rms += fx[i]*d; // sum of weights
 		}
-		rms_pure = Math.sqrt(rms)/this.pure_weight;
+		rms_pure = Math.sqrt(rms)/this.pure_weight; // Not Math.sqrt(rms/this.pure_weight)? 
 		for (int i = num_samples; i < num_points; i++) {
 			double d = (values[i] - fx[i]);
 			fx[i] = this.weights[i] * d;