First working version of IMU/LWIR positioning fusion (7de7fe06) · Commits · Elphel / imagej-elphel

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

+93 −1

Original line number	Original line	Diff line number	Diff line
	@@ -27,6 +27,9 @@ import java.util.ArrayList;
	import java.util.Arrays;		import java.util.Arrays;
	import java.util.concurrent.atomic.AtomicInteger;		import java.util.concurrent.atomic.AtomicInteger;

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

	import com.elphel.imagej.calibration.CalibrationFileManagement;		import com.elphel.imagej.calibration.CalibrationFileManagement;
	import com.elphel.imagej.cameras.CLTParameters;		import com.elphel.imagej.cameras.CLTParameters;
	import com.elphel.imagej.cameras.ColorProcParameters;		import com.elphel.imagej.cameras.ColorProcParameters;
	@@ -2068,7 +2071,7 @@ public class Interscene {

	// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).		// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
	// double avg_rlen		// double avg_rlen
	readjust_xy_ims = false;		// readjust_xy_ims = false;

	if (lpf_xy && (reg_weight_xy > 0.0)) {		if (lpf_xy && (reg_weight_xy > 0.0)) {
	scenes_xyzatr_pull = QuadCLT.refineFromLPF(		scenes_xyzatr_pull = QuadCLT.refineFromLPF(
	@@ -2082,6 +2085,62 @@ public class Interscene {
	earliest_scene, // int early_index,		earliest_scene, // int early_index,
	last_scene); // int last_index)		last_scene); // int last_index)
	} else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {		} else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
			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_scene //(quadCLTs.length -1) // int last_index,
			);
			double [] rms = new double[5];
			double [] quat = new double[4];
			int quat_lma_mode = 2; // 1; // 2;
			int debug_lev = debugLevel; // 3;
			// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
			double translation_weight = 1.0 / (avg_z + 1.0);

			//scenes_xyzatr

			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,
			scenes_xyzatr, // double [][][] xyzatr,
			pimu_xyzatr, // double [][][] ims_xyzatr,
			ref_index, // int ref_index,
			earliest_scene, // int early_index,
			last_scene, // 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) {
			System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() failed");
			} else {
			System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() finished in "+((int) rms[4])+ "iterations.");
			System.out.println("rms="+rms[0]+" ("+rms[2]+"), pure rms="+rms[1]+" ("+rms[3]+")");
			if (debug_lev > -3) {
			Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
			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]+"]");
			}

			// TODO: Use quat[] to update corrections of the IMU relative to the camera?
			for (int nscene = earliest_scene; nscene <= last_scene; nscene++) if (scenes_xyzatr[nscene] != null) {
			scenes_xyzatr_pull[nscene] = modifyATRtoXYZ(
			scenes_xyzatr[nscene], // double [][] cur_xyzatr, // careful with Z - using the new one
			rotated_xyzatr[nscene][0], // double [] new_xyz,
			avg_z); // double avg_z
			}
			}
			// old version
			/*
	scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(		scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
	clt_parameters, // CLTParameters clt_parameters,		clt_parameters, // CLTParameters clt_parameters,
	quadCLTs, // QuadCLT[] quadCLTs,		quadCLTs, // QuadCLT[] quadCLTs,
	@@ -2090,6 +2149,7 @@ public class Interscene {
	ref_index, // int ref_index,		ref_index, // int ref_index,
	earliest_scene, // int early_index,		earliest_scene, // int early_index,
	last_scene); // int last_index)		last_scene); // int last_index)
			*/
	}		}
	if (copy_pull_current) { // freeze_xy_pull) {		if (copy_pull_current) { // freeze_xy_pull) {
	System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");		System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
	@@ -2560,6 +2620,30 @@ public class Interscene {
	return earliest_scene;		return earliest_scene;
	}		}

			/**
			* Modify initial ATR to match change of the XYZ from the already adjusted pose.
			* Uses the fact that movement in X is approximate equivalent to Azimuth rotation
			* (positive X has the same effect as negative A multiplied by Z) and Y-movement
			* has the same sign to Tilt.
			* @param cur_xyzatr current (already adjusted by the image-based LMA) {{x,y,z},{a,t,r}
			* @param new_xyz new (target from the IMS) {x,y,z}
			* @param avg_z average altitude (Z)
			* @return new pose {{x,y,z},{a,t,r} expected to have a close match between scenes
			*/
			public static double [][] modifyATRtoXYZ(
			double [][] cur_xyzatr, // careful with Z - using the new one
			double [] new_xyz,
			double avg_z
			){
			double dx = new_xyz[0] - cur_xyzatr[0][0];
			double dy = new_xyz[1] - cur_xyzatr[0][1];
			// System.out.println(String.format("modifyATRtoXYZ(): dx=%7.3f, dy=%7.3f", dx, dy));
			double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
			double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
			return new_xyzatr;
			}


	// Make it the only entry point		// Make it the only entry point
	// uses *_dt from		// uses *_dt from
	// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()		// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()
	@@ -2683,7 +2767,15 @@ public class Interscene {
	true); // boolean rectilinear)		true); // boolean rectilinear)
	if (debugLevel > -3) {		if (debugLevel > -3) {
	System.out.println("adjustDiffPairsLMAInterscene(): pull_offset="+pull_offset);		System.out.println("adjustDiffPairsLMAInterscene(): pull_offset="+pull_offset);
			double dx = scene1_xyzatr_pull[0][0]-scene1_xyzatr[0][0];
			double dy = scene1_xyzatr_pull[0][1]-scene1_xyzatr[0][1];
			double da = (scene1_xyzatr_pull[1][0]-scene1_xyzatr[1][0])*average_z;
			double dt = (scene1_xyzatr_pull[1][1]-scene1_xyzatr[1][1])*average_z;
			// System.out.println(String.format("modifyATRtoXYZ(): dx=%8.4f, dy=%8.4f"+
			// ", hda=%8.4f, hdt=%8.4f, dx+hda=%8.4f, dy-hdt=%8.4f"
			// , dx, dy, da, dt, dx+da, dy-dt));
	}		}

	while (pull_offset > clt_parameters.imp.max_pull_jump) {		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,		// 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		// but if yes - quaternions are needed. Note, that angles are updated and will not now match pull

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

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -5437,7 +5437,7 @@ public class OpticalFlow {
	}		}
	double [] rms = new double[5];		double [] rms = new double[5];
	double [] quat = new double[4];		double [] quat = new double[4];
	int quat_lma_mode = 1; // 2;		int quat_lma_mode = 2; // 1;
	int debug_lev = 3;		int debug_lev = 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/QuadCLTCPU.java

+8 −4

Original line number	Original line	Diff line number	Diff line
	@@ -1098,6 +1098,8 @@ public class QuadCLTCPU {
	boolean use_rot,		boolean use_rot,
	boolean rectilinear		boolean rectilinear
	) {		) {
			double [][] xyzatr0_a = new double [][] {xyzatr0[0].clone(),xyzatr0[1].clone()}; // debugging
			double [][] xyzatr1_a = new double [][] {xyzatr1[0].clone(),xyzatr1[1].clone()};
	ErsCorrection ers = getErsCorrection();		ErsCorrection ers = getErsCorrection();
	double disp_avg = ers.getDisparityFromZ(average_z);		double disp_avg = ers.getDisparityFromZ(average_z);
	int [] wh = ers.getSensorWH();		int [] wh = ers.getSensorWH();
	@@ -1117,17 +1119,19 @@ public class QuadCLTCPU {
	xy[1], // double py, // pixel coordinate Y in the reference view		xy[1], // double py, // pixel coordinate Y in the reference view
	disp_avg, // double disparity, // this reference disparity		disp_avg, // double disparity, // this reference disparity
	!rectilinear, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear		!rectilinear, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
	xyzatr0[0], // double [] reference_xyz, // this view position in world coordinates (typically zero3)		xyzatr0_a[0], // double [] reference_xyz, // this view position in world coordinates (typically zero3)
	xyzatr0[1], // double [] reference_atr, // this view orientation relative to world frame (typically zero3)		xyzatr0_a[1], // double [] reference_atr, // this view orientation relative to world frame (typically zero3)
	!rectilinear, // boolean distortedCamera, // camera view is distorted (false - rectilinear)		!rectilinear, // boolean distortedCamera, // camera view is distorted (false - rectilinear)
	xyzatr1[0], // double [] camera_xyz, // camera center in world coordinates		xyzatr1_a[0], // double [] camera_xyz, // camera center in world coordinates
	xyzatr1[1], // double [] camera_atr, // camera orientation relative to world frame		xyzatr1_a[1], // double [] camera_atr, // camera orientation relative to world frame
	OpticalFlow.LINE_ERR); // double line_err); // threshold error in scan lines (1.0)		OpticalFlow.LINE_ERR); // double line_err); // threshold error in scan lines (1.0)
	if (pXpYD != null) {		if (pXpYD != null) {
	double dx = pXpYD[0]-xy[0]; // null pointer		double dx = pXpYD[0]-xy[0]; // null pointer
	double dy = pXpYD[1]-xy[1];		double dy = pXpYD[1]-xy[1];
	s2 += dxdx+dydy;		s2 += dxdx+dydy;
	s0+=1.0;		s0+=1.0;
			// System.out.println(String.format("estimateAverageShift(): dx=%8.4f, dy=%8.4f",dx, dy));

	} else {		} else {
	continue;		continue;
	}		}

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

+7 −5

Original line number	Original line	Diff line number	Diff line
	@@ -599,7 +599,8 @@ public class QuaternionLma {
	final double q1 = vector[1];		final double q1 = vector[1];
	final double q2 = vector[2];		final double q2 = vector[2];
	final double q3 = vector[3];		final double q3 = vector[3];
	final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal		// final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
			final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
	if (jt != null) {		if (jt != null) {
	for (int i = 0; i < vector.length; i++) {		for (int i = 0; i < vector.length; i++) {
	jt[i] = new double [weights.length];		jt[i] = new double [weights.length];
	@@ -1231,7 +1232,9 @@ public class QuaternionLma {
	double [] q,		double [] q,
	double [] r) {		double [] r) {
	return new double [][] {		return new double [][] {
			// for inverted {-q0,q1,q2,q3}
	// t=s*q' d/dQ0		// t=s*q' d/dQ0
			/*
	{ 2r[0]q[0],		{ 2r[0]q[0],
	2r[1]q[0] + 2r[2]q[3] - 2r[3]q[2],		2r[1]q[0] + 2r[2]q[3] - 2r[3]q[2],
	2r[2]q[0] + 2r[3]q[1] - 2r[1]q[3],		2r[2]q[0] + 2r[3]q[1] - 2r[1]q[3],
	@@ -1251,8 +1254,8 @@ public class QuaternionLma {
	-2r[3]q[1] - 2r[2]q[0] + 2r[1]q[3],		-2r[3]q[1] - 2r[2]q[0] + 2r[1]q[3],
	-2r[3]q[2] + 2r[1]q[0] + 2r[2]q[3],		-2r[3]q[2] + 2r[1]q[0] + 2r[2]q[3],
	-2r[1]q[1] - 2r[2]q[2] - 2r[3]q[3]}		-2r[1]q[1] - 2r[2]q[2] - 2r[3]q[3]}
			*/
	/*		// for non-inverted {q0,q1,q2,q3}
	// t=s*q' d/dQ0		// t=s*q' d/dQ0
	{ 2r[0]q[0],		{ 2r[0]q[0],
	2r[1]q[0] + 2r[2]q[3] - 2r[3]q[2],		2r[1]q[0] + 2r[2]q[3] - 2r[3]q[2],
	@@ -1273,7 +1276,6 @@ public class QuaternionLma {
	2r[3]q[1] + 2r[2]q[0] - 2r[1]q[3],		2r[3]q[1] + 2r[2]q[0] - 2r[1]q[3],
	2r[3]q[2] - 2r[1]q[0] - 2r[2]q[3],		2r[3]q[2] - 2r[1]q[0] - 2r[2]q[3],
	2r[1]q[1] + 2r[2]q[2] + 2r[3]q[3]}		2r[1]q[1] + 2r[2]q[2] + 2r[3]q[3]}
	*/
	};		};
	}		}
	public static double [][] composeQR_QdQ_(		public static double [][] composeQR_QdQ_(

Admin message