Implemented IMU-only scene sequence orientation (8406092c) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

+2 −0

Original line number	Diff line number	Diff line
		@@ -5086,6 +5086,8 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
		}
		QUAD_CLT_AUX.showExtrinsicCorr("aux");// show_fine_corr("aux");
		QUAD_CLT_AUX.geometryCorrection.showRig();// show_fine_corr("aux");
		System.out.println("=== IMS ===");
		QUAD_CLT_AUX.showQuatCorr();

		@SuppressWarnings("unused")
		QuadCLT dbg_QUAD_CLT = QUAD_CLT;

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

+100 −7

Original line number	Diff line number	Diff line
		@@ -444,13 +444,34 @@ public class Interscene {
		}

		boolean scale_ims_velocities = false; // true;
		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms(
		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // does not depend on correction
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		scale_ims_velocities); // boolean scale)

		double [] quat_corr = compensate_ims_rotation? quadCLTs[ref_index].getQuatCorr() : null; //
		// quat_corr may still be null if does not exist

		if (debugLevel > -3) {
		System.out.println("setInitialOrientationsIms(): compensate_ims_rotation="+compensate_ims_rotation);
		System.out.println("setInitialOrientationsIms(): will "+((quat_corr==null)? "NOT ":"")+" correct orientation offset");
		QuadCLTCPU.showQuatCorr(quat_corr);
		}
		boolean test_quat_corr = debugLevel > 1000;
		if (test_quat_corr) {
		testPIMU (
		clt_parameters, // final CLTParameters clt_parameters,
		quadCLTs, // final QuadCLT[] quadCLTs,
		ref_index, // final int ref_index,
		debugLevel); // final int debugLevel);
		/*
		quadCLTs[ref_index].getXyzatrIms(
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		null, // double [] quat_corr, // only applies to rotations - verify!
		debugLevel) ; // int debugLevel)
		*/
		}
		double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		@@ -797,6 +818,49 @@ public class Interscene {
		return earliest_scene;
		}

		public static void testPIMU(
		final CLTParameters clt_parameters,
		final QuadCLT[] quadCLTs,
		final int ref_index,
		final int debugLevel
		) {
		double [][][] ims_xyzatr = quadCLTs[ref_index].getXyzatrIms(
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		null, // double [] quat_corr, // only applies to rotations - verify!
		debugLevel) ; // int debugLevel)
		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
		clt_parameters, // final CLTParameters clt_parameters,
		quadCLTs, // final QuadCLT[] quadCLTs,
		ref_index, // final int ref_index,
		0, // final int early_index,
		(quadCLTs.length -1), // int last_index,
		(new double[2][3]) //final double [][] offsets // null - will not subtract from velocities
		);
		double [][][] dxyzatr = QuadCLT.getDxyzatrPIMU(
		clt_parameters,
		quadCLTs,
		false);
		double [] timestamps = new double [quadCLTs.length];
		for (int nscene = 0; nscene < quadCLTs.length; nscene++) {
		timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
		}

		System.out.println("N\tTimestamp\timsX\timsY\timsZ\timsA\timsT\timsR"+
		"\tpimuX\tpimuY\tpimuZ\tpimuA\tpimuT\tpimuR"+
		"\tvelX\tvelY\tvelZ\tomegaA\tomegaT\tomegaR");
		for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
		System.out.println(
		nscene+"\t"+timestamps[nscene]+"\t"+
		ims_xyzatr[nscene][0][0]+ "\t"+ims_xyzatr[nscene][0][1]+ "\t"+ims_xyzatr[nscene][0][2]+"\t"+
		ims_xyzatr[nscene][1][0]+ "\t"+ims_xyzatr[nscene][1][1]+ "\t"+ims_xyzatr[nscene][1][2]+"\t"+
		pimu_xyzatr[nscene][0][0]+"\t"+pimu_xyzatr[nscene][0][1]+"\t"+pimu_xyzatr[nscene][0][2]+"\t"+
		pimu_xyzatr[nscene][1][0]+"\t"+pimu_xyzatr[nscene][1][1]+"\t"+pimu_xyzatr[nscene][1][2]+"\t"+
		dxyzatr[nscene][0][0]+ "\t"+dxyzatr[nscene][0][1]+ "\t"+dxyzatr[nscene][0][2]+"\t"+
		dxyzatr[nscene][1][0]+ "\t"+dxyzatr[nscene][1][1]+ "\t"+dxyzatr[nscene][1][2]+"\t");
		}
		System.out.println();
		}

		public static int [][] getFPNPairs(
		ArrayList<Integer> fpn_list,
		@@ -1413,7 +1477,9 @@ public class Interscene {
		double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
		boolean debug_ers = clt_parameters.ilp.ilma_debug_ers ; // true;
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // for offset estimation

		if (debugLevel > -3) {
		System.out.println("reAdjustPairsLMAInterscene(): avg_z="+avg_z);
		}
		String dbg_ers_string =
		((dbg_scale_dt[1][0] > 0)?"a":((dbg_scale_dt[1][0] < 0) ? "A":"0"))+
		((dbg_scale_dt[1][1] > 0)?"t":((dbg_scale_dt[1][1] < 0) ? "T":"0"))+
		@@ -2042,11 +2108,30 @@ public class Interscene {
		while (pull_offset > clt_parameters.imp.max_pull_jump) {
		// so far assuming only X and Y to be modified. Z - won't harm, angles are not used now,
		// but if yes - quaternions are needed. Note, that angles are updated and will not now match pull
		// here updtae all parameters that are NOT adjusted. For angles use individual angles, just avoid
		// here update all parameters that are NOT adjusted. For angles use individual angles, just avoid
		// crossing 2pi
		/// XYZ ErsCorrection.DP_DSX + i
		for (int i = 0; i < 3; i++) if (!param_select[ErsCorrection.DP_DSX + i]){
		scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
		}
		// Az [-pi,+pi), Tl [-pi/2,+pi/2), Rl[-pi.+pi], but only roll can cross +/-pi
		for (int i = 0; i < 3; i++) if (!param_select[ErsCorrection.DP_DSAZ + i]){
		if (i==2) {
		double da = scene1_xyzatr_pull[1][i] - scene1_xyzatr[1][i];
		if (da > Math.PI) da -= 2 * Math.PI;
		else if (da < -Math.PI) da += 2 * Math.PI;
		scene1_xyzatr[0][i] += da * clt_parameters.imp.max_pull_jump / pull_offset;
		if (scene1_xyzatr[1][i] >= Math.PI) scene1_xyzatr[1][i] -= 2 * Math.PI;
		else if (scene1_xyzatr[1][i] < -Math.PI) scene1_xyzatr[1][i] += 2 * Math.PI;
		} else {
		scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
		}
		}
		/*
		for (int i = 0; i < 2; i++) {
		scene1_xyzatr[0][i] += (scene1_xyzatr_pull[0][i] - scene1_xyzatr[0][i]) * clt_parameters.imp.max_pull_jump / pull_offset;
		}
		*/
		// apply small scene1_xyzatr jump towards scene1_xyzatr_pull, run adjustPairsLMAInterscene, return if failed
		double [][] new_xyzatr = adjustPairsLMAInterscene( // assumes reference GPU data is already set - once for multiple scenes
		clt_parameters, // CLTParameters clt_parameters,
		@@ -2086,8 +2171,16 @@ public class Interscene {
		if (new_pull_offset < pull_offset) {
		scene1_xyzatr = new_xyzatr;
		// later - watch which parameters are updated by LMA, update those
		scene1_xyzatr_pull[1] = new_xyzatr[1]; // update adjusted orientation
		scene1_xyzatr_pull[0][2] = new_xyzatr[0][2];// update Z
		for (int i = 0; i < 3; i++) {
		if (param_select[ErsCorrection.DP_DSX + i]){
		scene1_xyzatr_pull[0][i] = scene1_xyzatr[0][i];
		}
		if (param_select[ErsCorrection.DP_DSAZ + i]){
		scene1_xyzatr_pull[1][i] = scene1_xyzatr[1][i];
		}
		}
		// scene1_xyzatr_pull[1] = new_xyzatr[1]; // update adjusted orientation
		// scene1_xyzatr_pull[0][2] = new_xyzatr[0][2];// update Z
		pull_offset = new_pull_offset;
		if (debugLevel > -3) {
		System.out.println("adjustDiffPairsLMAInterscene(): new pull_offset="+pull_offset);
		@@ -4442,7 +4535,7 @@ public class Interscene {
		ims_ortho);
		double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
		double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrIms( // velocities and omegas from IMU
		double [][][] scenes_dxyzatr = QuadCLTCPU.getDxyzatrPIMU( // velocities and omegas from IMU
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		false); // boolean scale)

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

+13 −6

Original line number	Diff line number	Diff line
		@@ -57,15 +57,15 @@ public class IntersceneMatchParameters {
		public double sfm_min_gain = 5.0; // Minimal SfM gain to apply SfM to the depth map
		public double sfm_min_frac = 0.5; // Minimal fraction of defined tiles to have SfM correction

		public int sfm_num_pairs = 16; // desired number of SfM pairs to average
		public int sfm_num_pairs = 32; // desired number of SfM pairs to average
		public double sfp_tolerance = 0.05; // average SfM pairs if their baselines differ less
		public int sfm_readjust = 5; // number of SfM readjustment cycles
		public double sfm_prev_frac = 0.6; // update if new sfm gain > this fraction of the old one
		public double sfm_same_weight = 0.8; // correction weight when new SfM gain is the same as the old one
		public int sfm_shrink = 2; // shrink sfm gain area before applying sfm_fade_sigma
		public double sfm_fade_sigma = 3.0; // fade SfM gains at the edges
		public double sfm_min_str1 = 0.2; // update if correction strength exceeds
		public double sfm_min_str16 = 0.4; // update if correction strength exceeds (for >=16 pairs)
		public double sfm_min_str1 = 0.15; // 0.22; // update if correction strength exceeds
		public double sfm_min_str16 = 0.22; // 0.4; // update if correction strength exceeds (for >=16 pairs)
		public boolean sfm_use_neibs = true; // use neighbors if individual corr is too weak
		public double sfm_neib_too_str1= 0.3; // do not count neighbors stronger than that
		public double sfm_neib_too_str16= 0.4; // do not count neighbors stronger than that (for >=16 pairs)
		@@ -95,6 +95,7 @@ public class IntersceneMatchParameters {

		public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
		public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
		public boolean use_quat_corr = true; // Use orientation correction everywhere if available

		public boolean generate_egomotion = true; // generate egomotion table (image-based and ims)
		public boolean generate_mapped = true; // generate any of the sequences - Tiff, video, mono or stereo
		@@ -572,7 +573,9 @@ public class IntersceneMatchParameters {
		gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
		"Calculate camera orientation correction from predicted by IMS .");
		gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
		"Apply camera orientation correction from predicted by IMS.");
		"Apply camera orientation correction from predicted by IMS when generating output files.");
		gd.addCheckbox ("Use IMS orientation correction internally", this.use_quat_corr,
		"Use camera orientation correction from predicted by IMS internally from the previous calculations.");

		gd.addMessage ("Generate/show scene sequences");
		gd.addCheckbox ("Generate egomotion table", this.generate_egomotion,
		@@ -1288,6 +1291,7 @@ public class IntersceneMatchParameters {
		this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
		this.calc_quat_corr = gd.getNextBoolean();
		this.apply_quat_corr = gd.getNextBoolean();
		this.use_quat_corr = gd.getNextBoolean();
		this.generate_egomotion = gd.getNextBoolean();
		this.generate_mapped = gd.getNextBoolean();
		this.reuse_video = gd.getNextBoolean();
		@@ -1683,6 +1687,7 @@ public class IntersceneMatchParameters {
		properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
		properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
		properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
		properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
		properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean
		properties.setProperty(prefix+"generate_mapped", this.generate_mapped+""); // boolean
		properties.setProperty(prefix+"reuse_video", this.reuse_video+""); // boolean
		@@ -2042,6 +2047,7 @@ public class IntersceneMatchParameters {
		if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
		if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
		if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
		if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
		if (properties.getProperty(prefix+"generate_egomotion")!=null) this.generate_egomotion=Boolean.parseBoolean(properties.getProperty(prefix+"generate_egomotion"));
		if (properties.getProperty(prefix+"generate_mapped")!=null) this.generate_mapped=Boolean.parseBoolean(properties.getProperty(prefix+"generate_mapped"));
		if (properties.getProperty(prefix+"reuse_video")!=null) this.reuse_video=Boolean.parseBoolean(properties.getProperty(prefix+"reuse_video"));
		@@ -2428,6 +2434,7 @@ public class IntersceneMatchParameters {
		imp.min_num_interscene = this.min_num_interscene;
		imp.calc_quat_corr = this.calc_quat_corr;
		imp.apply_quat_corr = this.apply_quat_corr;
		imp.use_quat_corr = this.use_quat_corr;
		imp.generate_egomotion = this.generate_egomotion;
		imp.generate_mapped = this.generate_mapped;
		imp.reuse_video = this.reuse_video;

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

+16 −16

Original line number	Diff line number	Diff line
		@@ -4656,7 +4656,9 @@ public class OpticalFlow {
		// skip completely if no color or mono, tiff or video

		boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
		boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
		// apply_quat_corr used inside getGroundIns() - used when generating output
		// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
		boolean use_ims_rotation = clt_parameters.imp.use_quat_corr; // use internally
		boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
		boolean generate_mapped = clt_parameters.imp.generate_mapped &&
		(gen_seq_mono_color[0] \|\| gen_seq_mono_color[1]); // generate sequences - Tiff and/or video
		@@ -4665,6 +4667,7 @@ public class OpticalFlow {
		boolean [] annotate_mono_color = {clt_parameters.imp.annotate_mono,clt_parameters.imp.annotate_color};
		boolean annotate_transparent_mono = clt_parameters.imp.annotate_transparent_mono;


		boolean [] generate_modes3d = {
		clt_parameters.imp.generate_raw,
		clt_parameters.imp.generate_inf,
		@@ -4980,14 +4983,13 @@ public class OpticalFlow {
		boolean ims_use = center_reference \|\| clt_parameters.imp.ims_use; // center works with IMS only
		/// int center_index=ref_index;
		int ref_index = last_index; // old versions
		boolean compensate_ims_rotation = false;
		if (force_initial_orientations && !reuse_video) {
		boolean OK = false;
		int es1 = -1;
		if (center_reference) {
		es1 = Interscene.setInitialOrientationsCenterIms(
		clt_parameters, // final CLTParameters clt_parameters,
		compensate_ims_rotation, // final boolean compensate_ims_rotation,
		use_ims_rotation, // final boolean compensate_ims_rotation,
		min_num_scenes, // int min_num_scenes,
		colorProcParameters, // final ColorProcParameters colorProcParameters,
		debayerParameters, // EyesisCorrectionParameters.DebayerParameters debayerParameters,
		@@ -5014,7 +5016,7 @@ public class OpticalFlow {
		} else if (ims_use) {
		earliest_scene = Interscene.setInitialOrientationsIms(
		clt_parameters, // final CLTParameters clt_parameters,
		compensate_ims_rotation, // final boolean compensate_ims_rotation,
		use_ims_rotation, // final boolean compensate_ims_rotation,
		min_num_scenes, // int min_num_scenes,
		colorProcParameters, // final ColorProcParameters colorProcParameters,
		quadCLTs, // final QuadCLT[] quadCLTs, //
		@@ -5265,16 +5267,15 @@ public class OpticalFlow {
		if (quadCLTs[ref_index].getNumOrient() < (min_num_orient - 1)) {
		mb_max_gain = clt_parameters.imp.mb_max_gain_inter;
		}
		// TODO: move to config
		// boolean freeze_xy_pull = true; // false; // true; // debugging freezing xy to xy_pull
		boolean configured_lma = false;
		boolean lpf_xy = false; // lpf x and y, re-adjust X,Y,Z,A,T,R with pull for X,Y. Disables
		double avg_rlen = clt_parameters.imp.avg_len; // 3.0;
		boolean readjust_xy_ims = true; // false;
		double reg_weight_xy = 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values


		boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
		boolean ers_from_ims = true; // false; // change later
		// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
		int ers_mode = 0; // keep
		// with IMS it is already set during initial orientation. In non-IMS mode
		if (!ers_from_ims && (quadCLTs[ref_index].getNumOrient() < 2)) {
		@@ -5427,8 +5428,7 @@ public class OpticalFlow {
		if (debugLevel> -3) {
		System.out.println("LMA done on iteration "+num_iter+
		" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
		System.out.println("Correction quaternion = ["+quatCorr[0]+", "+
		quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
		QuadCLTCPU.showQuatCorr(quatCorr);
		}
		quadCLTs[ref_index].setQuatCorr(quatCorr);
		quadCLT_main.setQuatCorr(quatCorr);

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

+108 −5

File changed.

Preview size limit exceeded, changes collapsed.