Next snapshot (0835c213) · Commits · Elphel / imagej-elphel

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

+22 −12

Original line number	Diff line number	Diff line
		@@ -39,6 +39,9 @@ import java.util.concurrent.atomic.AtomicBoolean;
		import java.util.concurrent.atomic.AtomicInteger;
		import java.util.concurrent.atomic.DoubleAccumulator;

		import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
		import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;

		import java.util.concurrent.ThreadLocalRandom;

		import com.elphel.imagej.calibration.CalibrationFileManagement;
		@@ -5434,8 +5437,16 @@ public class OpticalFlow {
		}
		double [] rms = new double[5];
		double [] quat = new double[4];
		int quat_lma_mode = 1; // 2;
		int debug_lev = 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,
		@@ -5444,10 +5455,9 @@ public class OpticalFlow {
		last_index, // int last_index,
		rms, //double [] rms, // null or double[5];
		quat, // double [] quaternion, // null or double[4]
		0); // int debugLevel
		}

		debug_lev); // int debugLevel

		}
		if (run_ly) {
		if (debugLevel > -3) {
		System.out.println("** Running LY adjustments ***");

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

+80 −27

Original line number	Diff line number	Diff line
		@@ -242,8 +242,11 @@ public class QuadCLTCPU {
		* @return quaternion componets
		*/

		public static double [] getRotationFromXYZCameraIms(
		public static double [] getRotationFromXYZATRCameraIms(
		CLTParameters clt_parameters,
		int quat_lma_mode,
		double avg_height,
		double translation_weight,
		QuadCLT[] quadCLTs,
		double [][][] xyzatr,
		double [][][] ims_xyzatr,
		@@ -253,12 +256,6 @@ public class QuadCLTCPU {
		double [] rms, // null or double[5];
		int debugLevel
		) {
		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];
		}
		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;
		@@ -269,6 +266,44 @@ public class QuadCLTCPU {
		double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
		double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		QuaternionLma quaternionLma = new QuaternionLma();
		if (quat_lma_mode == 2) {
		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];
		}
		quaternionLma.prepareLMA(
		avg_height, // double avg_height,
		vect_x, // double [][][] vect_x,
		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) {
		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];
		}
		quaternionLma.prepareLMA(
		vect_x, // double [][][] vect_x,
		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,
		@@ -276,6 +311,8 @@ public class QuadCLTCPU {
		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)
		}

		int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
		lambda, // double lambda, // 0.1
		lambda_scale_good,// double lambda_scale_good,// 0.5
		@@ -307,6 +344,9 @@ public class QuadCLTCPU {

		public static double [][][] rotateImsToCameraXYZ(
		CLTParameters clt_parameters,
		int quat_lma_mode,
		double avg_height,
		double translation_weight,
		QuadCLT[] quadCLTs,
		double [][][] xyzatr,
		double [][][] ims_xyzatr,
		@@ -317,8 +357,11 @@ public class QuadCLTCPU {
		double [] quaternion, // null or double[4]
		int debugLevel
		) {
		double [] quat = getRotationFromXYZCameraIms(
		double [] quat = getRotationFromXYZATRCameraIms(
		clt_parameters, // CLTParameters clt_parameters,
		quat_lma_mode, // int quat_lma_mode,
		avg_height, // double avg_height,
		translation_weight, // double translation_weight,
		quadCLTs, // QuadCLT[] quadCLTs,
		xyzatr, // double [][][] xyzatr,
		ims_xyzatr, // double [][][] ims_xyzatr,
		@@ -336,17 +379,27 @@ public class QuadCLTCPU {
		Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
		double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS
		double [] rotated_xyz = new double[3];
		// double [] rotated_atr = new double[3];
		Rotation rot_invert = rot.revert();
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);
		Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
		xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
		Rotation rotation_atr = rot.applyTo(scene_atr);
		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?
		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
		rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		}
		if (debugLevel > -1) {
		double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double [] degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
		System.out.println("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
		System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");

		System.out.println(String.format("%3s"+
		"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
		"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+

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

+968 −18

File changed.

Preview size limit exceeded, changes collapsed.

Admin message