Added linear velocities scale adjustment (5a110e1d) · Commits · Elphel / imagej-elphel

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

+2 −1

Original line number	Original line	Diff line number	Diff line
	@@ -2101,7 +2101,8 @@ public class Interscene {
	);		);
	double [] rms = new double[5];		double [] rms = new double[5];
	double [] quat = new double[4];		double [] quat = new double[4];
	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;		// int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
			int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
	int debug_lev = debugLevel; // 3;		int debug_lev = debugLevel; // 3;
	// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
	double translation_weight = 1.0 / (avg_z + 1.0);		double translation_weight = 1.0 / (avg_z + 1.0);

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

+47 −2

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

+139 −2

Original line number	Original line	Diff line number	Diff line
	@@ -467,6 +467,10 @@ public class QuadCLTCPU {
	boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted		boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted
	boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets		boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
	boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets		boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
			boolean adjust_accl = clt_parameters.imp.adjust_accl; // adjust IMU velocities scales
			boolean apply_accl = clt_parameters.imp.apply_accl; // apply IMU velocities scales
			double quat_max_change = clt_parameters.imp.quat_max_change; // do not apply if any component of the result exceeds
			double quat_min_lin = clt_parameters.imp.quat_min_lin; // meters, minimal distance per axis to adjust IMS velocity scale
	double [][][] pimu_xyzatr = QuadCLT.integratePIMU(		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
	clt_parameters, // final CLTParameters clt_parameters,		clt_parameters, // final CLTParameters clt_parameters,
	quadCLTs, // final QuadCLT[] quadCLTs,		quadCLTs, // final QuadCLT[] quadCLTs,
	@@ -486,7 +490,7 @@ public class QuadCLTCPU {
	int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;		int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
	int debug_lev = debugLevel; // 3;		int debug_lev = debugLevel; // 3;
	double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
	double translation_weight = 1.0 / (avg_z + 1.0);		double translation_weight = 0.0; // 1.0 / (avg_z + 1.0);
	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(		double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
	clt_parameters, // CLTParameters clt_parameters,		clt_parameters, // CLTParameters clt_parameters,
	quat_lma_mode, // int quat_lma_mode,		quat_lma_mode, // int quat_lma_mode,
	@@ -518,6 +522,17 @@ public class QuadCLTCPU {
	double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(		double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
	ims_mount_atr, // double [] ims_atr,		ims_mount_atr, // double [] ims_atr,
	quat); // double [] corr_q)		quat); // double [] corr_q)
			if (apply_imu_orient) {
			for (int i = 0; i < new_ims_mount_atr.length; i++) {
			if (Math.abs(new_ims_mount_atr[i]) > quat_max_change) {
			apply_imu_orient = false;
			if (debugLevel > -3) {
			System.out.println ("*** IMU mount angle ["+i+"]=="+new_ims_mount_atr[i]+" exceeds the specified limit ("+quat_max_change+")");
			System.out.println ("*** Orientation update is disabled.");
			}
			}
			}
			}
	if (apply_imu_orient) {		if (apply_imu_orient) {
	clt_parameters.imp.setImsMountATR(new_ims_mount_atr);		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
	if (debugLevel > -3) {		if (debugLevel > -3) {
	@@ -575,11 +590,133 @@ public class QuadCLTCPU {

	}		}
	}		}
			double [] acc_corr = null;
			if (adjust_accl) {
			acc_corr = getVelocitiesCorrections( // > 1 when IMU is larger than camera
			clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
			rotated_xyzatr, // double [][][] rotated_xyzatr,
			quadCLTs, // QuadCLT[] quadCLTs,
			ref_index, // int ref_index,
			earliest_scene, // int earliest_scene,
			last_index, // int last_index,
			quat_min_lin, // double min_range,
			debugLevel); // int debugLevel

			if (acc_corr != null) {
			double [] used_accl_corr = clt_parameters.imp.get_pimu_offs()[0];
			double [] new_accl_corr = used_accl_corr.clone();
			int num_corr=0;
			for (int i = 0; i < 3; i++) if (!Double.isNaN(acc_corr[i])){
			new_accl_corr[i] = used_accl_corr[i] * acc_corr[i];
			num_corr++;
			}
			if (debugLevel > -1) {
			System.out.println(String.format(
			"Used velocities scales = [%9f, %9f, %9f]", used_accl_corr[0],used_accl_corr[1],used_accl_corr[2]));
			System.out.println(String.format(
			"Diff.velocities scales = [%9f, %9f, %9f]", acc_corr[0], acc_corr[1], acc_corr[2]));
			System.out.println(String.format(
			"New velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
			}
			if (apply_accl && (num_corr>0)) {
			clt_parameters.imp.set_pimu_velocities_scales(new_accl_corr);
			if (debugLevel > -3) {
			System.out.println(String.format(
			"Applied new velocities scales = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
			System.out.println ("* Need to save the main configuration file *");
			}
			} else {
			if (debugLevel > -3) {
			System.out.println(String.format(
			"New velocities scales are not applied = [%9f, %9f, %9f]", new_accl_corr[0], new_accl_corr[1], new_accl_corr[2]));
			}
			}
			} else {
			System.out.println("* Adjustment of the gyro omegas failed *");

			}
			}
	} else {		} else {
	System.out.println ("* Failed to calculate IMS mount correction! *");		System.out.println ("* Failed to calculate IMS mount correction! *");
	}		}
	}		}

			public static double [] getVelocitiesCorrections(
			CLTParameters clt_parameters, // CLTParameters clt_parameters,
			double [][][] rotated_xyzatr,
			QuadCLT[] quadCLTs,
			int ref_index,
			int early_index,
			int last_index,
			double min_range,
			int debugLevel
			) {
			double [][][] xyzatr = new double [quadCLTs.length][][];
			ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
			double [] timestamps = new double [quadCLTs.length];
			double [][][] data = new double[3][last_index-early_index+1][2];
			// double ts_ref = quadCLTs[ref_index].getTimeStamp();
			double [][] xyz_minmax = new double [3][2]; // zeros OK as it is for ref scene
			for (int nscene = early_index; nscene <= last_index; nscene++) {
			timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
			xyzatr[nscene] = ers_ref.getSceneXYZATR(quadCLTs[nscene].getImageName());
			for (int i = 0; i < 3; i++) {
			// double d = xyzatr[nscene][0][i] - rotated_xyzatr[nscene][0][i];
			xyz_minmax[i][0] = Math.min(xyz_minmax[i][0], xyzatr[nscene][0][i]);
			xyz_minmax[i][1] = Math.max(xyz_minmax[i][1], xyzatr[nscene][0][i]);
			}
			}
			boolean [] dir_ok = new boolean[3];
			for (int i = 0; i < 3; i++) {
			dir_ok[i] = (xyz_minmax[i][1]-xyz_minmax[i][0]) > min_range;
			}
			for (int nscene = early_index; nscene <= last_index; nscene++) {
			// double rel_ts = timestamps[nscene] - ts_ref;
			for (int i = 0; i < 3; i++) if (dir_ok[i] ){
			data[i][nscene-early_index][0] = xyzatr[nscene][0][i]; // for all xyz the same
			data[i][nscene-early_index][1] = rotated_xyzatr[nscene][0][i];
			}
			}
			PolynomialApproximation pa= new PolynomialApproximation(-1); // debugLevel);
			double [][] coeffs = new double [3][2];
			for (int i = 0; i < 3; i++) {
			if (dir_ok[i] ){
			coeffs[i] = pa.polynomialApproximation1d(data[i], 1); // linear
			} else {
			coeffs[i] = new double [] {Double.NaN,Double.NaN};
			}
			}
			if (debugLevel > -1) {
			System.out.println("Velocity X correction: " +coeffs[0][1]+" ("+coeffs[0][0]+")");
			System.out.println("Velocity Y correction: " +coeffs[1][1]+" ("+coeffs[1][0]+")");
			System.out.println("Velocity Z correction: " +coeffs[2][1]+" ("+coeffs[2][0]+")");
			if (debugLevel > 0) {
			System.out.println(String.format("%3s"+
			"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
			"N","X","Y","Z","IMU-X","IMU-Y","IMU-Z","lX","lY","lZ"));
			for (int nscene = early_index; nscene <= last_index; nscene++) {
			System.out.println(String.format("%3d"+
			"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
			nscene,
			xyzatr[nscene][0][0],
			xyzatr[nscene][0][1],
			xyzatr[nscene][0][2],
			rotated_xyzatr[nscene][0][0],
			rotated_xyzatr[nscene][0][1],
			rotated_xyzatr[nscene][0][2],
			coeffs[0][0]+coeffs[0][1] * xyzatr[nscene][0][0],
			coeffs[1][0]+coeffs[1][1] * xyzatr[nscene][0][1],
			coeffs[2][0]+coeffs[2][1] * xyzatr[nscene][0][2]));
			}
			}
			}



			return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};

			}

	public static double [] getOmegaCorrections(		public static double [] getOmegaCorrections(
	CLTParameters clt_parameters, // CLTParameters clt_parameters,		CLTParameters clt_parameters, // CLTParameters clt_parameters,
	double [][][] rotated_xyzatr,		double [][][] rotated_xyzatr,
	@@ -881,7 +1018,7 @@ public class QuadCLTCPU {
	double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};		double [][] dxyzatr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
	if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?		if (pimu_offsets != null) { // TODO: apply offsets directly to omegas?
	for (int i = 0; i < 3; i++) {		for (int i = 0; i < 3; i++) {
	dxyzatr[0][i] -= pimu_offsets[0][i];		dxyzatr[0][i] /= pimu_offsets[0][i];
	dxyzatr[1][i] -= pimu_offsets[1][i];		dxyzatr[1][i] -= pimu_offsets[1][i];
	}		}
	}		}

Original line number	Original line	Diff line number	Diff line
	@@ -46,6 +46,7 @@ public class IntersceneMatchParameters {
	// Tilt -> -pimu_omega[1] rad/s, average did_pimu.theta[1]/did_pimu.dt		// Tilt -> -pimu_omega[1] rad/s, average did_pimu.theta[1]/did_pimu.dt
	// Roll -> +pimu_omega[0] rad/s, average did_pimu.theta[0]/did_pimu.dt		// Roll -> +pimu_omega[0] rad/s, average did_pimu.theta[0]/did_pimu.dt
	public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418};		public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418};
			public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1)


	@Deprecated		@Deprecated
	@@ -117,6 +118,8 @@ public class IntersceneMatchParameters {
	public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted		public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
	public boolean adjust_gyro = false;// adjust qyro omegas offsets		public boolean adjust_gyro = false;// adjust qyro omegas offsets
	public boolean apply_gyro = true; // apply adjusted qyro omegas offsets		public boolean apply_gyro = true; // apply adjusted qyro omegas offsets
			public boolean adjust_accl = false;// adjust IMU velocities scales
			public boolean apply_accl = true; // apply IMU velocities scales

	public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS		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 apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
	@@ -459,6 +462,7 @@ public class IntersceneMatchParameters {
	public double quat_max_change = 0.1; // radians		public double quat_max_change = 0.1; // radians
	public double quat_min_transl = 5.0; // meters to adjust by translation		public double quat_min_transl = 5.0; // meters to adjust by translation
	public double quat_min_rot = 0.1; // radians to adjust by rotation		public double quat_min_rot = 0.1; // radians to adjust by rotation
			public double quat_min_lin = 10.0; // meters, minimal distance per axis to adjust IMS velocity scale

	public boolean stereo_merge = true;		public boolean stereo_merge = true;
	public int stereo_gap = 32; // pixels between right and left frames		public int stereo_gap = 32; // pixels between right and left frames
	@@ -528,7 +532,8 @@ public class IntersceneMatchParameters {

	public double [][] get_pimu_offs(){		public double [][] get_pimu_offs(){
	// TODO: use ims_ortho, ims_mount_atr		// TODO: use ims_ortho, ims_mount_atr
	return new double [][] {new double[3],		return new double [][] {
			{ pimu_acc_offs[0] + 1, pimu_acc_offs[1] + 1, pimu_acc_offs[2] + 1},
	{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}		{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
	}		}

	@@ -537,6 +542,14 @@ public class IntersceneMatchParameters {
	pimu_gyr_offs[1] = - atr[1];		pimu_gyr_offs[1] = - atr[1];
	pimu_gyr_offs[0] = atr[2];		pimu_gyr_offs[0] = atr[2];
	}		}

			public void set_pimu_velocities_scales(double [] vxyz) {