Found and fixed bug in ExtrinsicAdjustment introduced while converting (f801b9b5) · Commits · Elphel / imagej-elphel

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

+73 −14

Original line number	Original line	Diff line number	Diff line
	@@ -589,6 +589,12 @@ public class ExtrinsicAdjustment {
	"Initial_y-fX_after_moving_objects");		"Initial_y-fX_after_moving_objects");
	}		}

			if (debugLevel > 0) {
			printYminusFxWeight(
			this.last_ymfx,
			this.weights,
			"Initial_y-fX_after_moving_objects");
			}
	double lambda = 0.1;		double lambda = 0.1;
	double lambda_scale_good = 0.5;		double lambda_scale_good = 0.5;
	double lambda_scale_bad = 8.0;		double lambda_scale_bad = 8.0;
	@@ -705,13 +711,11 @@ public class ExtrinsicAdjustment {
	fx[indx0] = this.weights[indx0] * d;		fx[indx0] = this.weights[indx0] * d;
	rms += fx[indx0]*d; // sum of weights		rms += fx[indx0]*d; // sum of weights
	for (int cam = 0; cam < num_sensors; cam++) {		for (int cam = 0; cam < num_sensors; cam++) {
	int indx = indx0 + cam + 1;		int indx = indx0 + cam + 1; // index of dd component
	// d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + cam] - fx[indx]);
	d = (-measured_dsxy[cluster][indx_dd0 + cam] - fx[indx]);		d = (-measured_dsxy[cluster][indx_dd0 + cam] - fx[indx]);
	fx[indx] = this.weights[indx] * d;		fx[indx] = this.weights[indx] * d;
	rms += fx[indx] * d; // sum of weights		rms += fx[indx] * d; // sum of weights
	indx = indx0 + cam + 5; // nd		indx = indx0 + cam + num_sensors + 1; // // index of nd component
	// d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + cam] - fx[indx]);
	d = (-measured_dsxy[cluster][indx_nd0 + cam] - fx[indx]);		d = (-measured_dsxy[cluster][indx_nd0 + cam] - fx[indx]);
	fx[indx] = this.weights[indx] * d;		fx[indx] = this.weights[indx] * d;
	rms += fx[indx] * d; // sum of weights		rms += fx[indx] * d; // sum of weights
	@@ -2149,6 +2153,55 @@ public class ExtrinsicAdjustment {
	}		}


			public void printYminusFxWeight(
			double [] fx,
			double [] weights,
			String title) {
			if (fx == null) {
			fx = new double [weights.length];
			}
			int clusters = clustersX * clustersY;
			// String [] titles = {"Y", "-fX", "Y+fx", "Weight", "W*(Y+fx)", "Masked Y+fx"};
			System.out.println("===== "+title+" =====");
			// text output
			for (int mode = 0; mode < points_sample; mode++) {
			if (mode == 0) {
			System.out.println("Differential Disparity");
			} else if (mode <= num_sensors) {
			System.out.println("DD-"+(mode - 1));
			} else {
			System.out.println("ND-"+(mode - num_sensors - 1));
			}
			System.out.println( "clusterX clusterY Y -fX Y + fX Weight W*(Y+fX) Masked (Y + fX)");
			for (int cluster = 0; cluster < clusters; cluster++) {
			int clusterX = cluster % clustersX;
			int clusterY = cluster / clustersX;
			int indx = cluster * points_sample + mode;
			if ((measured_dsxy[cluster] != null) && (weights[indx] > 0.0)){
			double [] dbg_data = new double[6];
			if (mode ==0) {
			dbg_data[0] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
			dbg_data[1] = -fx[indx];
			dbg_data[2] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
			dbg_data[3] = weights[indx]*clusters;
			dbg_data[4] = weights[indx]clusters(measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx]);
			dbg_data[5] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
			} else {
			dbg_data[0] = measured_dsxy[cluster][indx_dd0 + mode - 1];
			dbg_data[1] = -fx[indx];
			dbg_data[2] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
			dbg_data[3] = weights[indx]*clusters;
			dbg_data[4] = weights[indx]clusters(measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx]);
			dbg_data[5] = measured_dsxy[cluster][indx_dd0 + mode - 1] + fx[indx];
			}
			System.out.println(String.format(" %3d %3d %8.5f %8.5f %8.5f %8.5f %8.5f %8.5f",
			clusterX, clusterY, dbg_data[0],dbg_data[1],dbg_data[2],dbg_data[3],dbg_data[4],dbg_data[5]));
			}
			}
			}
			}


	private void dbgXY(		private void dbgXY(
	CorrVector corr_vector,		CorrVector corr_vector,
	String title) {		String title) {
	@@ -2366,12 +2419,18 @@ public class ExtrinsicAdjustment {
	if (this.last_rms == null) { //first time, need to calculate all (vector is valid)		if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
	this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];		this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
	this.last_ymfx = getFx(corr_vector);		this.last_ymfx = getFx(corr_vector);
	if (debug_level > -2) { // temporary		if (debug_level > -1) { // temporary
	dbgYminusFxWeight(		dbgYminusFxWeight(
	this.last_ymfx,		this.last_ymfx,
	this.weights,		this.weights,
	"Initial_y-fX_after_moving_objects");		"Initial_y-fX_after_moving_objects");
	}		}
			if (debug_level > -1) { // temporary
			printYminusFxWeight(
			this.last_ymfx,
			this.weights,
			"Initial_y-fX_after_moving_objects");
			}

	if (last_ymfx == null) {		if (last_ymfx == null) {
	return null; // need to re-init/restart LMA		return null; // need to re-init/restart LMA
	@@ -2400,7 +2459,7 @@ public class ExtrinsicAdjustment {
	lambda, // *10, // temporary		lambda, // *10, // temporary
	this.last_jt)); // double [][] jt)		this.last_jt)); // double [][] jt)
	if (debug_level>2) {		if (debug_level>2) {
	System.out.println("JtJ + lambda*diag(JtJ");		System.out.println("JtJ + lambda*diag(JtJ), lambda = "+lambda);
	wjtjlambda.print(18, 6);		wjtjlambda.print(18, 6);
	double [] diag_sqrt = getWJtDiagSqrt(this.last_jt);		double [] diag_sqrt = getWJtDiagSqrt(this.last_jt);
	System.out.print("diag_sqrt={");		System.out.print("diag_sqrt={");
	@@ -2428,20 +2487,20 @@ public class ExtrinsicAdjustment {
	return rslt;		return rslt;
	}		}
	if (debug_level>2) {		if (debug_level>2) {
	System.out.println("(JtJ + lambda*diag(JtJ).inv()");		System.out.println("(JtJ + lambda*diag(JtJ).inv(), lambda = "+lambda);
	jtjl_inv.print(18, 6);		jtjl_inv.print(18, 10);
	}		}
	//last_jt has NaNs		//last_jt has NaNs
	Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);		Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
	if (debug_level>2) {		if (debug_level>2) {
	System.out.println("Jt * (y-fx)");		System.out.println("Jt * (y-fx)");
	jty.print(18, 6);		jty.print(18, 10);
	}		}

	Matrix mdelta = jtjl_inv.times(jty);		Matrix mdelta = jtjl_inv.times(jty);
	if (debug_level>2) {		if (debug_level>2) {
	System.out.println("mdelta");		System.out.println("mdelta");
	mdelta.print(18, 6);		mdelta.print(18, 10);
	}		}

	double [] delta = mdelta.getColumnPackedCopy();		double [] delta = mdelta.getColumnPackedCopy();

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

+3 −3

Original line number	Original line	Diff line number	Diff line
	@@ -3222,10 +3222,10 @@ public class ImageDttCPU {
	if (disp_dist_cons != null) {		if (disp_dist_cons != null) {
	System.out.println("disp_dist_cons[0]="+disp_dist_cons[0]+", disp_dist_cons[0]="+disp_dist_cons[0]+" (this tile - weighted average will be discarded)");		System.out.println("disp_dist_cons[0]="+disp_dist_cons[0]+", disp_dist_cons[0]="+disp_dist_cons[0]+" (this tile - weighted average will be discarded)");
	}		}
	(new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());		// (new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());
	// mcorr_comb_width, // combined correlation tile width		// mcorr_comb_width, // combined correlation tile width
	//mcorr_comb_height, // combined correlation tile full height		//mcorr_comb_height, // combined correlation tile full height
	(new ShowDoubleFloatArrays()).showArrays(corr_combo_all, mcorr_comb_width, mcorr_comb_height, "corr_combo_all_CX"+clustX+"-CY"+clustY);		// (new ShowDoubleFloatArrays()).showArrays(corr_combo_all, mcorr_comb_width, mcorr_comb_height, "corr_combo_all_CX"+clustX+"-CY"+clustY);
	}		}

	if (ixy != null) { //TODO - for CM use magic!		if (ixy != null) { //TODO - for CM use magic!
	@@ -3246,7 +3246,7 @@ public class ImageDttCPU {
	if (disp_str_combo != null) {		if (disp_str_combo != null) {
	System.out.println("disp_str_combo[0]="+disp_str_combo[1]+", disp_str_combo[0]="+disp_str_combo[1]);		System.out.println("disp_str_combo[0]="+disp_str_combo[1]+", disp_str_combo[0]="+disp_str_combo[1]);
	}		}
	(new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());		// (new ShowDoubleFloatArrays()).showArrays(corrs_cons, 15, 15, true, "corrs_cons_CX"+clustX+"-CY"+clustY,correlation2d.getCorrTitles());
	}		}

	lma2 = correlation2d.corrLMA2Single(		lma2 = correlation2d.corrLMA2Single(

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

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -9537,7 +9537,7 @@ if (debugLevel > -100) return true; // temporarily !
	inf_max, // double inf_max,		inf_max, // double inf_max,
	threadsMax, //final int threadsMax, // maximal number of threads to launch		threadsMax, //final int threadsMax, // maximal number of threads to launch
	updateStatus,// final boolean updateStatus,		updateStatus,// final boolean updateStatus,
	1); // debugLevelInner); // final int debugLevel)		debugLevelInner); // 1); // final int debugLevel)
	if (!ok) break;		if (!ok) break;
	} else {		} else {
	boolean ok = quadCLT_aux.extrinsicsCLTfromGT(		boolean ok = quadCLT_aux.extrinsicsCLTfromGT(

Admin message