LMA to adjust scene's presumably LMA-related affines (not tilt-related) (83ce74fe) · Commits · Elphel / imagej-elphel

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

0 → 100644

+713 −0

File added.

Preview size limit exceeded, changes collapsed.

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

+130 −73

Original line number	Original line	Diff line number	Diff line
	@@ -23,11 +23,10 @@

	package com.elphel.imagej.orthomosaic;		package com.elphel.imagej.orthomosaic;

	import java.awt.Point;
	import java.awt.Rectangle;		import java.awt.Rectangle;
	import java.io.IOException;		import java.io.IOException;
	import java.text.SimpleDateFormat;		import java.text.SimpleDateFormat;
	import java.util.ArrayList;		import java.util.Arrays;
	import java.util.Calendar;		import java.util.Calendar;

	import com.elphel.imagej.calibration.CalibrationFileManagement;		import com.elphel.imagej.calibration.CalibrationFileManagement;
	@@ -60,7 +59,7 @@ public class OrthoAltitudeMatch {
	boolean invert_q2a = false;		boolean invert_q2a = false;
	boolean test_quat = false;		boolean test_quat = false;
	boolean test_quat0 = false;		boolean test_quat0 = false;
	boolean test_quat2= true;		boolean test_quat2= false; // true;
	boolean use_degrees = true;		boolean use_degrees = true;
	boolean debug_tilts = true;		boolean debug_tilts = true;

	@@ -140,9 +139,22 @@ public class OrthoAltitudeMatch {
	centers); // double [][] centers)		centers); // double [][] centers)
	final int width = wh[0];		final int width = wh[0];
	// final int height = wh[1];		// final int height = wh[1];
			final int num_scenes = indices.length;
			final int num_pairs = condensed_pairs.length;
	// int num_pairs = 0; // available_pairs.length		// int num_pairs = 0; // available_pairs.length
	// ArrayList<Point> failed_pairs = new ArrayList<Point>();		// ArrayList<Point> failed_pairs = new ArrayList<Point>();
			double [] weights_scenes = new double [num_scenes];
			double [] weights_pairs = new double [num_pairs];
			Arrays.fill(weights_scenes, 1.0);
			Arrays.fill(weights_pairs, 1.0);
			double [][][] scene_tilts_pairs = new double [num_pairs][2][];
			double [][][] affine_pairs = new double [num_pairs][][];
			double [] flat_err = new double[num_pairs];
			double [] overlaps = new double [num_pairs];

			double tilt_err_threshold = 0.01; // reduce weight if larger
			double overlop_pow = 2.0; // squared
			double weight_pairs_k = 100.0;
	for (int npair = 0; npair < condensed_pairs.length; npair++) {		for (int npair = 0; npair < condensed_pairs.length; npair++) {
	int [] cpair = condensed_pairs[npair]; // index alt_multi		int [] cpair = condensed_pairs[npair]; // index alt_multi
	int [] ipair = {indices[cpair[0]], indices[cpair[1]]};		int [] ipair = {indices[cpair[0]], indices[cpair[1]]};
	@@ -218,13 +230,63 @@ public class OrthoAltitudeMatch {
	}		}
	}		}
	double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};		double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
	if (test_quat2) {		double [][] alt_datas = new double[3][];
	System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]+
	", invert_q2a="+invert_q2a+", invert_order="+invert_order+", invert_y="+invert_y);
	boolean [][] masks = new boolean[2][];		boolean [][] masks = new boolean[2][];
	double [][] alt_data5s = new double[2][];		double [][] alt_data5s = new double[2][];
	double [][] data_overlap = new double[2][];		double [][] data_overlap = new double[2][];
	double [][] alt_datas = new double[3][];
			// calculate individual tilts
			for (int ns = 0; ns < cpair.length; ns++) {
			data_overlap[ns] = OrthoMap.extractWoi(
			alt_multi[cpair[ns]], // final double [] data0,
			width, // final int width,
			woi_overlap); // Rectangle woi_in);

			for (int ntry = 0; ntry <= alt_refine; ntry++) {
			String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
			alt_data5s[ns] = OrthoMap.getPlane(
			data_overlap[ns], // final double [] data,
			masks[ns], // final boolean [] mask,
			weight, // final double [] weight,
			woi_overlap.width, // final int width,
			xy0, // final double [] xy0) {
			dbg_name);
			if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
			masks[ns] = OrthoMap.removeRelativeLowHigh (
			data_overlap[ns], // final double [] data,
			null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
			alt_abs_outliers, // final double abs_diff,
			alt_outliers, // final double rel_frac,
			alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
			woi_overlap.width,// final int width, // only used with ground_plane != null;
			num_bins); // final int num_bins)
			} else {
			break;
			}
			}
			alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
			}
			double tilt_err2 = 0;
			alt_datas[2] = new double [3];
			for (int i = 0; i < alt_datas[2].length; i++) {
			alt_datas[2][i] = alt_datas[0][i]+alt_data[i]; // simulated alt_datas[1]
			if (i < 2) {
			double dt = alt_datas[2][i]-alt_datas[1][i];
			tilt_err2+=dt*dt;
			}
			}
			scene_tilts_pairs[npair] = new double [][] {alt_datas[0], alt_datas[2]}; // {tilt0, tilt1 (synthetic)
			affine_pairs[npair] = pairwiseOrthoMatch.getAffine(); // contains scale + rot
			flat_err[npair] = Math.sqrt(tilt_err2);
			overlaps[npair] = pairwiseOrthoMatch.getOverlap();
			if (!test_quat2) {
			continue;
			}


			if (test_quat2) {
			System.out.println("***************** npair="+npair+": "+ipair[0]+" -> "+ipair[1]+
			", invert_q2a="+invert_q2a+", invert_order="+invert_order+", invert_y="+invert_y);
	double [][] affine_pair = pairwiseOrthoMatch.getAffine();		double [][] affine_pair = pairwiseOrthoMatch.getAffine();
	boolean remove_rs = false;		boolean remove_rs = false;
	if (remove_rs) {		if (remove_rs) {
	@@ -265,37 +327,6 @@ public class OrthoAltitudeMatch {
	SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw),
	SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};		SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[2], y_down_ccw)};

	// calculate individual tilts
	for (int ns = 0; ns < cpair.length; ns++) {
	data_overlap[ns] = OrthoMap.extractWoi(
	alt_multi[cpair[ns]], // final double [] data0,
	width, // final int width,
	woi_overlap); // Rectangle woi_in);

	for (int ntry = 0; ntry <= alt_refine; ntry++) {
	String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
	alt_data5s[ns] = OrthoMap.getPlane(
	data_overlap[ns], // final double [] data,
	masks[ns], // final boolean [] mask,
	weight, // final double [] weight,
	woi_overlap.width, // final int width,
	xy0, // final double [] xy0) {
	dbg_name);
	if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
	masks[ns] = OrthoMap.removeRelativeLowHigh (
	data_overlap[ns], // final double [] data,
	null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
	alt_abs_outliers, // final double abs_diff,
	alt_outliers, // final double rel_frac,
	alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
	woi_overlap.width,// final int width, // only used with ground_plane != null;
	num_bins); // final int num_bins)
	} else {
	break;
	}
	}
	alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
	}

	// alt_datas[0] = alt_datas[0].clone();		// alt_datas[0] = alt_datas[0].clone();
	double [] tilts0_mod = QuatUtils.manualFitTilt(		double [] tilts0_mod = QuatUtils.manualFitTilt(
	@@ -494,44 +525,10 @@ public class OrthoAltitudeMatch {
	if (test_quat) {		if (test_quat) {
	System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);		System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);

	boolean [][] masks = new boolean[2][];
	double [][] alt_data5s = new double[2][];
	double [][] data_overlap = new double[2][];
	double [][] alt_datas = new double[3][];
	double [] quat_diff = QuatUtils.tiltToQuaternion(		double [] quat_diff = QuatUtils.tiltToQuaternion(
	alt_data,		alt_data,
	y_down_ccw); // boolean y_down_ccw)		y_down_ccw); // boolean y_down_ccw)
	double [][] quats01 = new double [alt_datas.length][];		double [][] quats01 = new double [alt_datas.length][];
	for (int ns = 0; ns < cpair.length; ns++) {
	data_overlap[ns] = OrthoMap.extractWoi(
	alt_multi[cpair[ns]], // final double [] data0,
	width, // final int width,
	woi_overlap); // Rectangle woi_in);

	for (int ntry = 0; ntry <= alt_refine; ntry++) {
	String dbg_name = (dbg_planes && (ntry == alt_refine)) ? ("plane_approximation_"+ipair[ns]+"_"+npair) :null;
	alt_data5s[ns] = OrthoMap.getPlane(
	data_overlap[ns], // final double [] data,
	masks[ns], // final boolean [] mask,
	weight, // final double [] weight,
	woi_overlap.width, // final int width,
	xy0, // final double [] xy0) {
	dbg_name);
	if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
	masks[ns] = OrthoMap.removeRelativeLowHigh (
	data_overlap[ns], // final double [] data,
	null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
	alt_abs_outliers, // final double abs_diff,
	alt_outliers, // final double rel_frac,
	alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
	woi_overlap.width,// final int width, // only used with ground_plane != null;
	num_bins); // final int num_bins)
	} else {
	break;
	}
	}
	alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
	}
	alt_datas[2] = new double [3];		alt_datas[2] = new double [3];
	for (int i = 0; i < alt_datas[2].length; i++) {		for (int i = 0; i < alt_datas[2].length; i++) {
	alt_datas[2][i] = alt_datas[0][i]+alt_data[i];		alt_datas[2][i] = alt_datas[0][i]+alt_data[i];
	@@ -875,6 +872,66 @@ public class OrthoAltitudeMatch {
	debugLevel-4); // final int debugLevel)		debugLevel-4); // final int debugLevel)
	*/		*/
	}		}
			// double tilt_err_threshold = 0.01; // reduce weight if larger
			//overlop_pow = 2.0


			double max_flat_err = 0;//tilt_err_threshold
			for (int npair = 0; npair < num_pairs; npair++) {
			max_flat_err = Math.max(max_flat_err, flat_err[npair]);
			//weights_pairs
			weights_pairs[npair] = tilt_err_threshold/Math.max(tilt_err_threshold, flat_err[npair])*Math.pow(overlaps[npair], overlop_pow);
			}

			ERSTiltLMA ersTiltLMA = new ERSTiltLMA();
			ersTiltLMA.prepareLMA(
			indices, // int [] indices, // should all be used
			condensed_pairs, // int [][] cpairs,
			weights_scenes, // double [] weights_scenes, // sfm, number used?
			weights_pairs, // double [] weights_pairs, // from matching tilts(flatness) (and worst sfm, per-pair rmse)?
			weight_pairs_k, // double weight_pairs_k,
			scene_tilts_pairs, // double [][][] tilts, // [pair][scene(2)][tilt(2)]
			affine_pairs, // double [][][] affine_pairs,
			debugLevel); // int debug_level)
			double lambda = 0.1;
			double lambda_scale_good = 0.5;
			double lambda_scale_bad = 8.0;
			double lambda_max = 1000;
			boolean last_run = false;

			double rms_diff = 0.0001;
			int num_iter = 20;
			int lma_rslt=ersTiltLMA.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, // String dbg_prefix,
			debugLevel); // int debug_level)
			System.out.println("LMA -> "+lma_rslt);
			if (lma_rslt >= 0) {
			if (debugLevel > -3) {
			ersTiltLMA.printResults(use_degrees);
			}
			}
			System.out.println();
			if (debugLevel > 1) {
			for (int npair = 0; npair < ersTiltLMA.num_pairs; npair++) {
			double [][] pseudo_xy= ersTiltLMA.getPseudoXY(npair);
			QuatUtils.testPseudoAffineDiffAndDerivatives(
			pseudo_xy, // double [][] xy, // [scene][direction]
			ersTiltLMA.aff_tilts[npair], // double [][][] a_tilts,
			ersTiltLMA.aff_pairs_nosr[npair], // double [][] iaff_pair, // affine pair inversed
			invert_q2a); // boolean invert_q2a){ // invert result affines (to match "usual")
			System.out.println();
			}

			}

			//printResults(boolean degrees)
	if (orthoMapsCollection_path != null) {		if (orthoMapsCollection_path != null) {
	try {		try {
	orthoMapsCollection.writeOrthoMapsCollection(orthoMapsCollection_path);		orthoMapsCollection.writeOrthoMapsCollection(orthoMapsCollection_path);

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

+9 −0

Original line number	Original line	Diff line number	Diff line
	@@ -114,6 +114,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
	// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image		// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
	// coordinates relative to vert_meters		// coordinates relative to vert_meters
	public double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[], positive Y is down (as in images)		public double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[], positive Y is down (as in images)
			public transient double [][] ers_affine = new double[][] {{1,0},{0,1}}; // orientation only for remaining ERS, positive Y is down (as in images)
	public double orig_pix_meters;		public double orig_pix_meters;
	public double [] vert_meters; // offset of the image vertical in meters (scale-invariant), right (X) and down (Y)		public double [] vert_meters; // offset of the image vertical in meters (scale-invariant), right (X) and down (Y)
	public int orig_width;		public int orig_width;
	@@ -190,6 +191,14 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
	// pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();		// pairwise_matches = new HashMap<Double, PairwiseOrthoMatch>();
	}		}

			public double [][] getERSAffine(){
			return ers_affine;
			}
			public void setERSAffine(double [][] ers_affine) { // cloned
			// this.ers_affine = ers_affine;
			this.ers_affine = new double [][] {ers_affine[0].clone(),ers_affine[1].clone()};
			}

	double getEqualized(double d) {		double getEqualized(double d) {
	return d * equalize[0] + equalize[1];		return d * equalize[0] + equalize[1];
	}		}

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

+8 −0

Original line number	Original line	Diff line number	Diff line
	@@ -40,10 +40,18 @@ public class PairwiseOrthoMatch implements Serializable {
	public transient double overlap = 0.0;		public transient double overlap = 0.0;
	public transient double [] alt_data = null;		public transient double [] alt_data = null;
	public transient double [] equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]		public transient double [] equalize1to0 = {1,0}; // value1 = equalize2to1[0]*value2+equalize2to1[1]
			public transient double [] quat = null; // relative orientation of the second scene (ERS affine removed). Will be eventually saved
	// below - not saved/restored		// below - not saved/restored
	public transient boolean ok = false; // not saved/restored		public transient boolean ok = false; // not saved/restored
	public transient int [] nxy = null; // not saved, just to communicate for logging		public transient int [] nxy = null; // not saved, just to communicate for logging

			public double [] getQuaternion() {
			return quat;
			}
			public void setQuaternion(double [] quat) { // clone() by caller
			this.quat = quat;
			}

	// public PairwiseOrthoMatch() {}		// public PairwiseOrthoMatch() {}
	public double getOverlap() {		public double getOverlap() {
	return overlap;		return overlap;

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

+635 −32

File changed.

Preview size limit exceeded, changes collapsed.

Admin message