fixed LMA bug, commented thread_invariant for LMA (d87f98cc) · Commits · Elphel / imagej-elphel

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

+4 −2

Original line number	Diff line number	Diff line
		@@ -1389,6 +1389,7 @@ public class Interscene {
		{
		System.out.println("reAdjustPairsLMAInterscene(): using mb_max_gain="+mb_max_gain);
		boolean freeze_xy_pull = false; // true; // debugging freezing xy to xy_pull
		boolean copy_pull_current = true;
		final boolean[] param_select = configured_lma? clt_parameters.ilp.ilma_lma_select :
		ErsCorrection.getParamSelect(
		!freeze_xy_pull && (!readjust_xy_ims \|\| (reg_weight_xy != 0)), // false only in mode c): freeze X,Y// boolean use_XY
		@@ -1545,7 +1546,7 @@ public class Interscene {
		earliest_scene, // int early_index,
		last_scene); // int last_index)
		}
		if (freeze_xy_pull) {
		if (copy_pull_current) { // freeze_xy_pull) {
		System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
		for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
		if (scenes_xyzatr_pull[nscene] != null) {
		@@ -2261,13 +2262,14 @@ public class Interscene {
		ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
		*/
		}

		// TODO: save ers_scene.ers_watr_center_dt and ers_scene.ers_wxyz_center_dt
		intersceneLma.prepareLMA(
		camera_xyz0, // final double [] scene_xyz0, // camera center in world coordinates (or null to use instance)
		camera_atr0, // final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
		scene_xyz_pull, // final double [] scene_xyz_pull, // if both are not null, specify target values to pull to
		scene_atr_pull, // final double [] scene_atr_pull, //
		// reference atr, xyz are considered 0.0
		// reference atr, xyz are considered 0.0 not anymore?
		scene_QuadClt, // final QuadCLT scene_QuadClt,
		reference_QuadClt, // final QuadCLT reference_QuadClt,
		param_select_mod, // param_select, // final boolean[] param_select,

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

+155 −216

Original line number	Diff line number	Diff line
		@@ -58,16 +58,23 @@ public class IntersceneLma {
		private double [][] macrotile_centers = null; // (will be used to pull for regularization)
		private double infinity_disparity = 0.1; // treat lower as infinity
		private int num_samples = 0;
		///////////////////////////////////////////////////////////
		// thread_invariant is needed for LMA, otherwise even with the same parameter vector RMS may be slightly different
		// keeping thread-variant where it is done once per LMA (during setup) - in normalizeWeights() and setSamplesWeights()
		// See original code (with all switching between variant/invariant) in pre 12/17/2023
		///////////////////////////////////////////////////////////

		private boolean thread_invariant = true; // Do not use DoubleAdder, provide results not dependent on threads
		private int num_components = 2; // 2 for 2D matching only,3 - include disparity
		private double disparity_weight = 1.0; // relative weight of disparity errors

		public IntersceneLma(
		boolean thread_invariant,
		double disparity_weight
		) {
		this.thread_invariant = thread_invariant;
		this.num_components = (disparity_weight > 0) ? 3 : 2;
		this.disparity_weight = disparity_weight;
		this.thread_invariant= thread_invariant;
		}

		public int getNumComponents() {
		@@ -133,22 +140,6 @@ public class IntersceneLma {

		public String [] printOldNew(boolean allvectors, int w, int d) {
		return printOldNew(allvectors, 0, w, d);
		/*
		String fmt1 = String.format("%%%d.%df", w+2,d+2); // more for the differences
		ArrayList<String> lines = new ArrayList<String>();
		for (int n = ErsCorrection.DP_DVAZ; n < ErsCorrection.DP_NUM_PARS; n+=3) {
		boolean adj = false;
		for (int i = 0; i <3; i++) adj \|= par_mask[n+i];
		if (allvectors \|\| adj) {
		String line = printNameV3(n, false, w,d)+" (" + getCompareType()+
		" "+printNameV3(n, true, w,d)+")";
		line += ", diff_last="+String.format(fmt1, getV3Diff(n)[0]);
		line += ", diff_first="+String.format(fmt1, getV3Diff(n)[1]);
		lines.add(line);
		}
		}
		return lines.toArray(new String[lines.size()]);
		*/
		}

		public String [] printOldNew(boolean allvectors, int mode, int w, int d) {
		@@ -198,29 +189,16 @@ public class IntersceneLma {

		public String getCompareType(int mode) {
		boolean is_pull = (mode == 0) ? (parameters_pull != null) : (mode > 1);
		// return (parameters_pull != null)? "pull": "was";
		return is_pull? "pull": "was ";
		}

		public String printNameV3(int indx, boolean initial, int w, int d) {
		return printNameV3(indx, initial, 0, w, d);
		/*
		double [] full_vector = initial?
		(use_pull? getFullVector(parameters_pull) : backup_parameters_full):
		getFullVector(parameters_vector);
		double [] vector = new double[3];
		for (int i = 0; i <3; i++) {
		vector[i] = full_vector[indx + i];
		}
		String name = ErsCorrection.DP_VECTORS_NAMES[indx];
		return printNameV3(name, vector, w, d);
		*/
		}

		public String printNameV3(int indx, boolean initial, int mode, int w, int d) {
		// mode: 0 - auto, 1 - was, 2 - pull
		boolean use_pull = (mode == 0) ? (parameters_pull != null) : (mode > 1);
		// boolean use_pull = parameters_pull != null;
		double [] full_vector = initial?
		(use_pull? getFullVector(parameters_pull) : backup_parameters_full):
		getFullVector(parameters_vector);
		@@ -255,7 +233,7 @@ public class IntersceneLma {
		final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
		final double [] scene_xyz_pull, // if both are not null, specify target values to pull to
		final double [] scene_atr_pull, //
		// reference atr, xyz are considered 0.0
		// reference atr, xyz are considered 0.0 - not anymore?
		final QuadCLT scene_QuadClt,
		final QuadCLT reference_QuadClt,
		final boolean[] param_select,
		@@ -360,6 +338,7 @@ public class IntersceneLma {
		scene_QuadClt, // final QuadCLT scene_QuadClt,
		reference_QuadClt, // final QuadCLT reference_QuadClt,
		debug_level); // final int debug_level)
		// Why y_vector starts with initial value of fx???
		y_vector = fx.clone();
		for (int i = 0; i < vector_XYSDS.length; i++) {
		if (vector_XYSDS[i] != null){
		@@ -371,6 +350,13 @@ public class IntersceneLma {

		}
		}
		if (parameters_pull != null){
		for (int i = 0; i < par_indices.length; i++) {
		// y_vector [i + num_components * macrotile_centers.length] += parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
		y_vector [i + num_components * macrotile_centers.length] = parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
		}
		}

		last_rms = new double [2];
		last_ymfx = getYminusFxWeighted(
		fx, // final double [] fx,
		@@ -516,18 +502,6 @@ public class IntersceneLma {
		Matrix wjtjlambda = new Matrix(getWJtJlambda(
		lambda, // *10, // temporary
		this.last_jt)); // double [][] jt)
		if (debug_level > 3) {
		try {
		System.out.println("getFxDerivs(): getChecksum(this.y_vector)="+ getChecksum(this.y_vector));
		System.out.println("getFxDerivs(): getChecksum(this.weights)="+ getChecksum(this.weights));
		System.out.println("getFxDerivs(): getChecksum(this.last_ymfx)="+ getChecksum(this.last_ymfx));
		System.out.println("getFxDerivs(): getChecksum(y_minus_fx_weighted)="+getChecksum(y_minus_fx_weighted));
		System.out.println("getFxDerivs(): getChecksum(wjtjlambda)= "+getChecksum(wjtjlambda));
		} catch (NoSuchAlgorithmException \| IOException e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
		}
		}

		if (debug_level>2) {
		System.out.println("JtJ + lambda*diag(JtJ");
		@@ -554,16 +528,6 @@ public class IntersceneLma {
		System.out.println("Jt * (y-fx)");
		jty.print(18, 6);
		}
		if (debug_level > 2) {
		try {
		System.out.println("getFxDerivs(): getChecksum(jtjl_inv)="+getChecksum(jtjl_inv));
		System.out.println("getFxDerivs(): getChecksum(jty)= "+getChecksum(jty));
		} catch (NoSuchAlgorithmException \| IOException e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
		}
		}



		Matrix mdelta = jtjl_inv.times(jty);
		@@ -578,18 +542,6 @@ public class IntersceneLma {
		for (int i = 0; i < parameters_vector.length; i++) {
		new_vector[i] += scale * delta[i];
		}
		if (debug_level > 2) {
		try {
		System.out.println("getFxDerivs(): getChecksum(mdelta)= "+getChecksum(mdelta));
		System.out.println("getFxDerivs(): getChecksum(delta)= "+getChecksum(delta));
		System.out.println("getFxDerivs(): getChecksum(parameters_vector)= "+getChecksum(parameters_vector));
		System.out.println("getFxDerivs(): getChecksum(new_vector)= "+getChecksum(new_vector));
		} catch (NoSuchAlgorithmException \| IOException e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
		}
		}


		double [] fx = getFxDerivs(
		new_vector, // double [] vector,
		@@ -666,20 +618,21 @@ public class IntersceneLma {

		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final AtomicInteger ati = new AtomicInteger(0);
		final double [] sw_arr = new double [threads.length];
		double sum_weights;
		if (thread_invariant) {
		final int num_types = (num_components > 2) ? 2 : 1;
		final double [] sw_arr = new double [num_types * vector_XYSDS.length];
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		public void run() {
		int thread_num = ati.getAndIncrement();
		for (int iMTile = ai.getAndIncrement(); iMTile < vector_XYSDS.length; iMTile = ai.getAndIncrement()) if (vector_XYSDS[iMTile] != null){
		double w = vector_XYSDS[iMTile][2];
		if (Double.isNaN(w)) {
		w = 0;
		}
		weights[num_components * iMTile] = w;
		sw_arr[num_types * iMTile] = 2*w;
		sw_arr[thread_num] += 2*w;
		//disparity_weight
		if (num_types > 1) {
		w = vector_XYSDS[iMTile][4] * disparity_weight;
		@@ -688,7 +641,7 @@ public class IntersceneLma {
		vector_XYSDS[iMTile][3] = 0.0;
		}
		weights[num_components * iMTile + 2] = w;
		sw_arr[num_types * iMTile + 1] = w;
		sw_arr[thread_num] += w;
		}
		}
		}
		@@ -699,38 +652,11 @@ public class IntersceneLma {
		for (double w:sw_arr) {
		sum_weights += w;
		}
		} else {
		final DoubleAdder asum_weight = new DoubleAdder();
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		public void run() {
		for (int iMTile = ai.getAndIncrement(); iMTile < vector_XYSDS.length; iMTile = ai.getAndIncrement()) if (vector_XYSDS[iMTile] != null){
		double w = vector_XYSDS[iMTile][2];
		if (Double.isNaN(w)) {
		w = 0;
		}
		weights[num_components * iMTile] = w;
		asum_weight.add(w);
		w = vector_XYSDS[iMTile][4] * disparity_weight;
		if (Double.isNaN(w) \|\| Double.isNaN(vector_XYSDS[iMTile][3])) {
		w = 0;
		vector_XYSDS[iMTile][3] = 0.0;
		}
		weights[num_components * iMTile + 2] = w;
		asum_weight.add(2*w);
		}
		}
		};
		}
		ImageDtt.startAndJoin(threads);
		sum_weights = asum_weight.sum();
		}
		if (sum_weights <= 1E-8) {
		System.out.println("!!!!!! setSamplesWeights(): sum_weights=="+sum_weights+" <= 1E-8");
		}
		ai.set(0);
		// final double s = 0.5/sum_weights;
		final double s = 1.0/sum_weights; // already taken care of
		final double s = 1.0/sum_weights; // Was 0.5 - already taken care of
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		public void run() {
		@@ -752,51 +678,34 @@ public class IntersceneLma {
		{
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		double full_weight, sum_weight_pure;
		if (thread_invariant) {
		sum_weight_pure = 00;
		for (int i = 0; i < num_samples; i++) {
		if (Double.isNaN(weights[i])) {
		System.out.println("normalizeWeights(): weights["+i+"] == NaN: 1");
		weights[i] = 0;
		}
		sum_weight_pure += weights[i];
		}
		full_weight = sum_weight_pure;
		for (int i = 0; i < par_indices.length; i++) {
		int indx = num_samples + i;
		if (Double.isNaN(weights[indx])) {
		System.out.println("normalizeWeights(): weights["+indx+"] == NaN: 1.5, i="+i);
		weights[indx] = 0;
		}
		full_weight += weights[indx];
		}
		} else {
		final DoubleAdder asum_weight = new DoubleAdder();
		final AtomicInteger ati = new AtomicInteger(0);
		final double [] sum_weight = new double [threads.length];
		ati.set(0);
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		public void run() {
		int thread_num = ati.getAndIncrement();
		for (int i = ai.getAndIncrement(); i < num_samples; i = ai.getAndIncrement()){
		if (Double.isNaN(weights[i])) {
		System.out.println("normalizeWeights(): weights["+i+"== NaN: 2");
		weights[i] = 0;
		}
		asum_weight.add(weights[i]);
		sum_weight[thread_num] += weights[i];
		}
		}
		};
		}
		ImageDtt.startAndJoin(threads);
		sum_weight_pure = asum_weight.sum();
		double sum_weight_pure = 0;
		for (double sw:sum_weight) sum_weight_pure += sw;
		double full_weight = sum_weight_pure;
		for (int i = 0; i < par_indices.length; i++) {
		int indx = num_samples + i;
		if (Double.isNaN(weights[indx])) {
		System.out.println("normalizeWeights(): weights["+indx+"] == NaN: 1.5, i="+i);
		weights[indx] = 0;
		}
		asum_weight.add(weights[indx]);
		}
		full_weight = asum_weight.sum();
		full_weight += weights[indx];
		}
		pure_weight = sum_weight_pure/full_weight;
		final double s = 1.0/full_weight;
		@@ -939,25 +848,14 @@ public class IntersceneLma {
		}
		// pull to the initial parameter values
		for (int i = 0; i < par_indices.length; i++) {
		fx [i + num_components * macrotile_centers.length] = vector[i]; // - parameters_initial[i]; // scale will be combined with weights
		jt[i][i + num_components * macrotile_centers.length] = 1.0; // scale will be combined with weights
		}
		if (parameters_pull != null){
		for (int i = 0; i < par_indices.length; i++) {
		fx [i + num_components * macrotile_centers.length] -= parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
		}
		}
		if (debug_level > 3) {
		try {
		System.out.println ("getFxDerivs(): getChecksum(fx)="+getChecksum(fx));
		if (jt != null) {
		System.out.println("getFxDerivs(): getChecksum(jt)="+getChecksum(jt));
		}
		} catch (NoSuchAlgorithmException \| IOException e) {
		// TODO Auto-generated catch block
		e.printStackTrace();
		}
		}
		fx [i + num_samples] = vector[i]; // - parameters_initial[i]; // scale will be combined with weights
		jt[i][i + num_samples] = 1.0; // scale will be combined with weights
		}
		/// if (parameters_pull != null){
		/// for (int i = 0; i < par_indices.length; i++) {
		/// fx [i + num_samples] -= parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
		/// }
		/// }
		return fx;
		}

		@@ -1004,11 +902,20 @@ public class IntersceneLma {
		final double [] fx,
		final double [] rms_fp // null or [2]
		) {
		if (thread_invariant) {
		return getYminusFxWeightedInvariant(fx,rms_fp); // null or [2]
		} else {
		return getYminusFxWeightedFast (fx,rms_fp); // null or [2]
		}
		}
		private double [] getYminusFxWeightedInvariant(
		final double [] fx,
		final double [] rms_fp // null or [2]
		) {
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final double [] wymfw = new double [fx.length];
		double s_rms;
		if (thread_invariant) {
		final double [] l2_arr = new double [num_samples];
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		@@ -1032,23 +939,55 @@ public class IntersceneLma {
		for (double l2:l2_arr) {
		s_rms += l2;
		}
		} else {
		final DoubleAdder asum_weight = new DoubleAdder();
		double rms_pure = Math.sqrt(s_rms/pure_weight);
		for (int i = 0; i < par_indices.length; i++) {
		int indx = i + num_samples;
		double d = y_vector[indx] - fx[indx]; // fx[indx] == vector[i]
		double wd = d * weights[indx];
		s_rms += d * wd;
		wymfw[indx] = wd;
		}
		double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0; /pure_weight); shey should be re-normalized after adding regularization
		if (rms_fp != null) {
		rms_fp[0] = rms;
		rms_fp[1] = rms_pure;
		}
		return wymfw;
		}



		private double [] getYminusFxWeightedFast(
		final double [] fx,
		final double [] rms_fp // null or [2]
		) {
		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
		final AtomicInteger ai = new AtomicInteger(0);
		final double [] wymfw = new double [fx.length];
		final AtomicInteger ati = new AtomicInteger(0);
		final double [] l2_arr = new double [threads.length];
		for (int ithread = 0; ithread < threads.length; ithread++) {
		threads[ithread] = new Thread() {
		public void run() {
		for (int i = ai.getAndIncrement(); i < num_samples; i = ai.getAndIncrement()) {
		int thread_num = ati.getAndIncrement();
		for (int i = ai.getAndIncrement(); i < num_samples; i = ai.getAndIncrement()) if (weights[i] > 0) {
		double d = y_vector[i] - fx[i];
		double wd = d * weights[i];
		double l2 = d * wd;
		if (Double.isNaN(wd)) {
		System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
		", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
		}
		//double l2 = d * wd;
		l2_arr[thread_num] += d * wd;
		wymfw[i] = wd;
		asum_weight.add(l2);
		}
		}
		};
		}
		ImageDtt.startAndJoin(threads);
		s_rms = asum_weight.sum();
		double s_rms = 0.0;
		for (double l2:l2_arr) {
		s_rms += l2;
		}
		double rms_pure = Math.sqrt(s_rms/pure_weight);
		for (int i = 0; i < par_indices.length; i++) {

Admin message