working snapshot (working on IMU-to-camera rotation calibration) (76f96ef8) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/ims/Imx5.java

+1 −1

Original line number	Diff line number	Diff line
		@@ -249,7 +249,7 @@ public class Imx5 {
		* Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
		* @param ims_atr original ATR of the IMS mount
		* @param corr_q correction quaternion
		* @return corrected ATR of teh IMU mount
		* @return corrected ATR of the IMU mount
		*/
		public static double [] adjustMountAtrByQuat(
		double [] ims_atr,

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

+8 −8

Original line number	Diff line number	Diff line
		@@ -5181,7 +5181,7 @@ public class Interscene {
		debugLevel); // int debugLevel
		if (debugLevel > -3) {
		Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
		System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
		System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
		double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double [] corr_degrees = new double[3];
		for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
		@@ -5476,20 +5476,20 @@ public class Interscene {
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		QuadCLT ref_scene = quadCLTs[ref_index];
		ErsCorrection ers_reference = ref_scene.getErsCorrection();
		double [][] quat_lma_xyz = new double [quadCLTs.length][3];
		double [][] quat_lma_enu_xyz = new double [quadCLTs.length][3];
		double [][][] quat_lma_xyzatr = new double [quadCLTs.length][2][3];
		double [][][] quat_lma_enu_xyzatr = new double [quadCLTs.length][2][3];
		Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
		QuadCLT scene = quadCLTs[nscene];
		if (nscene == ref_index) {
		quat_lma_xyz[nscene] = new double[3];
		quat_lma_xyzatr[nscene][0] = new double[3];
		} else {
		String ts = scene.getImageName();
		quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
		quat_lma_xyzatr[nscene][0] = ers_reference.getSceneXYZ(ts);
		}
		Did_ins_2 d2 = scene.did_ins_2;
		double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
		quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
		quat_lma_enu_xyzatr[nscene][0] = Imx5.applyQuaternionTo(
		Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
		ims_mount_atr,
		ims_ortho),
		@@ -5508,8 +5508,8 @@ public class Interscene {
		int debug_level = 1;
		QuaternionLma quaternionLma = new QuaternionLma();
		quaternionLma.prepareLMA(
		quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
		quat_lma_xyz, // double [][] vect_y,
		quat_lma_enu_xyzatr, // quat_lma_xyz, // double [][] vect_x,
		quat_lma_xyzatr, // double [][] vect_y,
		null, // double [][] vect_w, all same weight
		reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
		quat0, // double [] quat0,

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

+8 −63

Original line number	Diff line number	Diff line
		@@ -5425,68 +5425,13 @@ public class OpticalFlow {
		}
		// later move to the right place
		if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
		clt_parameters, // final CLTParameters clt_parameters,
		quadCLTs, // final QuadCLT[] quadCLTs,
		ref_index, // final int ref_index,
		null, // double [][][] dxyzatr,
		earliest_scene, // final int early_index,
		last_index //(quadCLTs.length -1) // int last_index,
		);
		double [][][] xyzatr = new double [quadCLTs.length][][];
		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
		for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
		String ts = quadCLTs[nscene].getImageName();
		xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
		}
		double [] rms = new double[5];
		double [] quat = new double[4];
		int quat_lma_mode = 03; // 4; // 3; // 2; // 1;
		int debug_lev = debugLevel; // 3;
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
		double translation_weight = 1.0 / (avg_z + 1.0);

		double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
		clt_parameters, // CLTParameters clt_parameters,
		quat_lma_mode, // int quat_lma_mode,
		avg_z, // double avg_height,
		translation_weight, // double translation_weight,
		QuadCLT.adjustImuOrient(
		clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT[] quadCLTs,
		xyzatr, // double [][][] xyzatr,
		pimu_xyzatr, // double [][][] ims_xyzatr,
		ref_index, // int ref_index,
		earliest_scene, // int early_index,
		earliest_scene, // int earliest_scene,
		last_index, // int last_index,
		rms, //double [] rms, // null or double[5];
		quat, // double [] quaternion, // null or double[4]
		debug_lev); // int debugLevel
		if (rotated_xyzatr != null) {
		Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
		if (debugLevel > -3) {
		System.out.println("Applying correction ot the IMS mount orientation:");
		double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double [] corr_degrees = new double[3];
		for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
		System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
		System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
		}
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
		ims_mount_atr, // double [] ims_atr,
		quat); // double [] corr_q)
		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
		if (debugLevel > -3) {
		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
		System.out.println ("* Need to save the main configuration file *");
		}
		} else {
		System.out.println ("* Failed to calculate IMS mount correction! *");
		}
		debugLevel); // int debugLevel
		}
		if (run_ly) {
		if (debugLevel > -3) {
		@@ -5602,7 +5547,7 @@ public class OpticalFlow {
		debugLevel+1);
		if (debugLevel > -3) {
		Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
		System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
		System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
		double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double [] corr_degrees = new double[3];
		for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;

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

+112 −26

Original line number	Diff line number	Diff line
		@@ -245,6 +245,7 @@ public class QuadCLTCPU {
		public static double [] getRotationFromXYZATRCameraIms(
		CLTParameters clt_parameters,
		int quat_lma_mode,
		boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
		double avg_height,
		double translation_weight,
		QuadCLT[] quadCLTs,
		@@ -256,7 +257,7 @@ public class QuadCLTCPU {
		double [] rms, // null or double[5];
		int debugLevel
		) {
		final boolean use3 = (quat_lma_mode == 3); // true;
		// final boolean use3 = (quat_lma_mode == 3); // true;
		double lambda = clt_parameters.imp.quat_lambda; // 0.1;
		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
		double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
		@@ -267,49 +268,50 @@ public class QuadCLTCPU {
		double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
		double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		QuaternionLma quaternionLma = new QuaternionLma();
		if ((quat_lma_mode == 2) \|\| (quat_lma_mode == 4)) {
		double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
		double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		vect_x[nscene] = ims_xyzatr[nscene];
		vect_y[nscene] = xyzatr[nscene];
		}
		double [][][] vect_y_scaled = QuaternionLma.scaleXYZ(
		vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
		vect_y, // double [][][] vect_y, // []{{x,y,z},{a,t,r}}
		new int [] {early_index, last_index}); // int [] first_last){
		if ((quat_lma_mode == quaternionLma.MODE_COMBO) \|\| (quat_lma_mode == QuaternionLma.MODE_COMBO_LOCAL)) {
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		avg_height, // double avg_height,
		vect_x, // double [][][] vect_x,
		vect_y, // double [][][] vect_y,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [] vect_w, all same weight
		translation_weight, // double translation_weight, // 0.0 ... 1.0;
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		} else if ((quat_lma_mode == 1) \|\| (quat_lma_mode == 3)) {
		double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
		double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		vect_x[nscene] = ims_xyzatr[nscene];
		vect_y[nscene] = xyzatr[nscene];
		}
		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		vect_x, // double [][][] vect_x,
		vect_y, // double [][][] vect_y,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [] vect_w, all same weight
		translation_weight, // double translation_weight, // 0.0 ... 1.0;
		reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		} else {
		/*
		double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
		double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		vect_x[nscene] = ims_xyzatr[nscene][0];
		vect_y[nscene] = xyzatr[nscene][0];
		}

		*/
		quaternionLma.prepareLMA(
		vect_x, // quat_lma_xyz, // double [][] vect_x,
		vect_y, // double [][] vect_y,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [][] vect_w, all same weight
		reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
		quat0, // double [] quat0,
		@@ -360,9 +362,12 @@ public class QuadCLTCPU {
		double [] quaternion, // null or double[4]
		int debugLevel
		) {
		final boolean use3 = true; // false; // (quat_lma_mode == 3); // true;// extract from clt ?
		boolean use_inv = false; //
		double [] quat = getRotationFromXYZATRCameraIms(
		clt_parameters, // CLTParameters clt_parameters,
		quat_lma_mode, // int quat_lma_mode,
		use3, // boolean use3, // false - full4 quaternion (with scale), true - only q1,q2,q3
		avg_height, // double avg_height,
		translation_weight, // double translation_weight,
		quadCLTs, // QuadCLT[] quadCLTs,
		@@ -390,10 +395,19 @@ public class QuadCLTCPU {
		Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
		ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]);
		// Rotation rotation_atr = rot.applyTo(scene_atr);
		Rotation rotation_atr = rot.applyTo(ims_atr);
		Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
		// Rotation rotation_atr = rot.applyTo(ims_atr);
		// Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
		Rotation rotation_atr,rotation_atr2;
		if (use_inv) {
		rotation_atr = rot_invert.applyTo(ims_atr);
		rotation_atr2 = rotation_atr.applyTo(rot); // applyInverseTo?
		} else {
		rotation_atr = rot.applyTo(ims_atr);
		rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
		}
		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
		rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		/// rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		}
		if (debugLevel > -1) {
		double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		@@ -427,6 +441,78 @@ public class QuadCLTCPU {
		return rotated_xyzatr;
		}

		public static void adjustImuOrient(
		CLTParameters clt_parameters, // CLTParameters clt_parameters,
		QuadCLT[] quadCLTs,
		int ref_index,
		int earliest_scene,
		int last_index,
		int debugLevel
		) {
		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
		clt_parameters, // final CLTParameters clt_parameters,
		quadCLTs, // final QuadCLT[] quadCLTs,
		ref_index, // final int ref_index,
		null, // double [][][] dxyzatr,
		earliest_scene, // final int early_index,
		last_index //(quadCLTs.length -1) // int last_index,
		);
		double [][][] xyzatr = new double [quadCLTs.length][][];
		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
		for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
		String ts = quadCLTs[nscene].getImageName();
		xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
		}
		double [] rms = new double[5];
		double [] quat = new double[4];
		int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
		int debug_lev = debugLevel; // 3;
		double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
		double translation_weight = 1.0 / (avg_z + 1.0);
		double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
		clt_parameters, // CLTParameters clt_parameters,
		quat_lma_mode, // int quat_lma_mode,
		avg_z, // double avg_height,
		translation_weight, // double translation_weight,
		quadCLTs, // QuadCLT[] quadCLTs,
		xyzatr, // double [][][] xyzatr,
		pimu_xyzatr, // double [][][] ims_xyzatr,
		ref_index, // int ref_index,
		earliest_scene, // int early_index,
		last_index, // int last_index,
		rms, //double [] rms, // null or double[5];
		quat, // double [] quaternion, // null or double[4]
		debug_lev); // int debugLevel
		if (rotated_xyzatr != null) {
		Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
		if (debugLevel > -3) {
		System.out.println("Applying correction to the IMS mount orientation:");
		double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double [] corr_degrees = new double[3];
		double quat_scale = Math.sqrt(quat[0]quat[0]+quat[1]quat[1]+quat[2]quat[2]+quat[3]quat[3]);
		for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
		System.out.println("scale="+quat_scale);
		System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
		System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
		}
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
		ims_mount_atr, // double [] ims_atr,
		quat); // double [] corr_q)
		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
		if (debugLevel > -3) {
		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
		System.out.println ("* Need to save the main configuration file *");
		}
		} else {
		System.out.println ("* Failed to calculate IMS mount correction! *");
		}
		}

		/**
		* Refine scene poses (now only linear) from currently adjusted poses

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

+513 −98

File changed.

Preview size limit exceeded, changes collapsed.