Updated initial orientation, started refining position (e17a0659) · Commits · Elphel / imagej-elphel

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

+1216 −351

File changed.

Preview size limit exceeded, changes collapsed.

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

+3 −0

Original line number	Diff line number	Diff line
		@@ -4975,12 +4975,14 @@ 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,
		min_num_scenes, // int min_num_scenes,
		colorProcParameters, // final ColorProcParameters colorProcParameters,
		debayerParameters, // EyesisCorrectionParameters.DebayerParameters debayerParameters,
		@@ -5007,6 +5009,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,
		min_num_scenes, // int min_num_scenes,
		colorProcParameters, // final ColorProcParameters colorProcParameters,
		quadCLTs, // final QuadCLT[] quadCLTs, //

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

+199 −25

Original line number	Diff line number	Diff line
		@@ -125,6 +125,7 @@ public class QuadCLTCPU {

		public static final boolean USE_PRE_2021 = false; // temporary
		public static final int THREADS_MAX = 100;
		public static final double [] ZERO3 = {0.0,0.0,0.0};

		// public GPUTileProcessor.GpuQuad gpuQuad = null;

		@@ -205,7 +206,90 @@ public class QuadCLTCPU {
		public void setQuatCorr(double[] quat) {
		quat_corr = quat;
		}
		public double [][] getDxyzatrIms(

		/**
		* Refine scene poses (now only linear) from currently adjusted poses
		* (adjusted using IMS orientations) and IMS velocities (currently
		* only linear are used). Refined scene poses may be used as pull targets
		* with free orientations (and angular velocities for ERS).
		* Result poses are {0,0,0} for the reference frame.
		*
		* Assuming correct IMU angular velocities and offset linear ones.
		*
		* @param clt_parameters configuration parameters
		* @param quadCLTs scenes sequence (for timestamps)
		* @param xyzatr current scenes [[x,y,z],[a,t,r]] in reference scene frame
		* @param dxyzatr IMU-provided linear and angular velocities in reference
		* scene frame
		* @param ref_index reference scene index
		* @param early_index earliest (lowest) scene index to use
		* @param last_index last (highest) scene index to use
		* @return refined array of scene poses [[x,y,z],[a,t,r]] (in reference scene frame),
		* same indices as input
		*/
		public static double [][][] refineFromImsVelocities(
		CLTParameters clt_parameters,
		QuadCLT[] quadCLTs,
		double [][][] xyzatr,
		double [][][] dxyzatr,
		int ref_index,
		int early_index,
		int last_index
		) {
		double [][] xyz_integ = new double [quadCLTs.length][3];
		double [] tim = new double [quadCLTs.length];
		double t00= quadCLTs[early_index].getTimeStamp();
		double t0 = t00;
		double s0 = 0, sx = 0, sx2 = 0;
		double [] sxy = new double[3], sy = new double[3];
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		double t = quadCLTs[nscene].getTimeStamp();
		double x = t - t00; // from early_index
		tim[nscene] = x;
		s0 +=1.0;
		sx += x;
		sx2 += x*x;
		double dt = (t-t0)/2; // half from previous
		t0 = t;
		for (int i = 0; i < 3; i++) {
		double y = 0;
		if (nscene > early_index) {
		xyz_integ[nscene][i] = xyz_integ[nscene - 1][i]+dt * (dxyzatr[nscene-1][0][i] + dxyzatr[nscene-1][0][i]);
		y = xyzatr[nscene][0][i] - xyz_integ[nscene][i];
		}
		sy[i] += y;
		sxy[i] += x*y;
		}
		}
		double denom = sx2 * s0 - sx * sx;
		double [] a = new double[3], b = new double[3], ref_offs = new double[3];
		for (int i = 0; i < 3; i++) {
		a[i] = (sxy[i] * s0 - sx * sy[i])/denom;
		b[i] = (sy[i] * sx2 - sx * sxy[i])/denom;
		ref_offs[i] = b[i] + a[i] * tim[ref_index] + xyz_integ[ref_index][i];
		}
		double [][][] xyzatr_out = new double [quadCLTs.length][][];
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		xyzatr_out[nscene] = new double[3][2];
		for (int i = 0; i < 3; i++) {
		xyzatr_out[nscene][0][i] = b[i] + a[i] * tim[nscene] + xyz_integ[nscene][i] - ref_offs[i];
		xyzatr_out[nscene][1][i] = xyzatr[nscene][1][i]; // for now - just copy old, maybe will add smth.
		}
		}

		return xyzatr_out;
		}


		/**
		* Get linear and angular velocities (camera frame) from
		* this scene IMS data
		* @param clt_parameters configuration parameters (including
		* scales from IMS to linear for linear and angular velocities
		* @param scale if true, multiply by scales from he configuration parameters)
		* @return linear and angular velocities in camera frame
		*/
		protected double [][] getDxyzatrIms(
		CLTParameters clt_parameters,
		boolean scale) {
		double [] ims_ortho = clt_parameters.imp.ims_ortho;
		@@ -219,16 +303,106 @@ public class QuadCLTCPU {
		double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false);
		double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
		// a (around Y),t (around X), r (around Z)
		double [][] dxyzatyr = 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 (scale) {
		for (int i = 0; i < 3; i++) {
		dxyzatyr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
		dxyzatyr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
		dxyzatr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
		dxyzatr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
		}
		}
		return dxyzatyr;
		return dxyzatr;
		}

		/**
		* Get linear and angular velocities (camera frame) from
		* scenes' IMS data
		* @param clt_parameters configuration parameters (including
		* scales from IMS to linear for linear and angular velocities
		* @param quadCLTs array of scenes with IMS data loaded
		* @param scale if true, multiply by scales from he configuration parameters)
		* @return linear and angular velocities in camera frame for each scene that has IMS data
		*/
		public static double [][][] getDxyzatrIms(
		CLTParameters clt_parameters,
		QuadCLT[] quadCLTs,
		boolean scale) {
		double [][][] dxyzatr = new double [quadCLTs.length][][];
		for (int i = 0; i < quadCLTs.length; i++) if (quadCLTs[i] != null) {
		dxyzatr[i] = quadCLTs[i].getDxyzatrIms(
		clt_parameters,
		scale);
		}
		return dxyzatr;
		}


		/**
		* Get offset and rotation of each scene from the sequence relative to this
		* scene using IMS data. Linear offsets may be too big for the fix, so they
		* need running correction from the last matched scene, scenes should start from
		* the closest to this one and move gradually farther away.
		* @param clt_parameters Configuration parameters, including IMS to camera rotation
		* @param quadCLTs array of scenes
		* @param quat_corr optional quaternion correcting IMS orientation (when known) or null
		* @param debugLevel debug level
		* @return per-scene array of [[x,y,z],[a,t,r]] in camera frame for initial fitting.
		* Orientation may be later used to pull LMA. Result for reference (this) scene may
		* be small values, but not exactly zeros.
		*/
		public double [][][] getXyzatrIms(CLTParameters clt_parameters,
		QuadCLT[] quadCLTs,
		double [] quat_corr, // only applies to rotations - verify!
		int debugLevel){
		double [] ims_ortho = clt_parameters.imp.ims_ortho;
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
		// scenes_xyzatr[ref_index] = new double[2][3]; // all zeros

		Did_ins_2 d2_ref = did_ins_2;
		// apply correction to orientation - better not to prevent extra loops.
		// And it only applies to rotations
		double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
		ims_mount_atr,
		ims_ortho);
		if (quat_corr != null) {
		cam_quat_ref_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_ref_enu, false);
		if (debugLevel > -3) {
		System.out.println("getXyzatrIms(): Applying attitude correction from quaternion ["+
		quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"]");
		}
		}
		double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
		double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
		double [][] last_corr_xyzatr = {ZERO3,ZERO3};
		for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {
		QuadCLT scene = quadCLTs[nscene];
		Did_ins_2 d2 = scene.did_ins_2;
		double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up
		double [] cam_quat_enu =Imx5.quaternionImsToCam(
		d2.getQEnu(),
		ims_mount_atr,
		ims_ortho);
		if (quat_corr != null) {
		cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
		}
		double [] cam_xyz_enu = Imx5.applyQuaternionTo(
		// Offset in the reference scene frame, not in the current scene
		cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
		enu, // absolute offset (East, North, Up) in meters
		false);
		double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
		double [][] ims_scene_xyzatr_enu = {cam_xyz_enu, scene_abs_atr_enu }; // try with xyz?
		// set initial approximation from IMS, subtract reference XYZATR
		// predicted by IMU from the reference scene
		double [][] pose_ims = ErsCorrection.combineXYZATR(
		ims_scene_xyzatr_enu,
		ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu));
		scenes_xyzatr[nscene] = pose_ims;
		}
		return scenes_xyzatr;
		}


		public double [] getLla() {
		return did_ins_2.lla;
		}
		@@ -355,10 +529,6 @@ public class QuadCLTCPU {
		scene1.getImageName());
		}

		//QuadCLTCPU [] scenes
		// TODO: Use dsi[] instead
		// @Deprecated
		// public boolean[] blue_sky = null;
		public void inc_orient() {num_orient++;}
		public void inc_accum() {num_accum++;}
		public void set_orient(int num) {num_orient = num;}
		@@ -417,30 +587,20 @@ public class QuadCLTCPU {
		/**
		* Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
		* should have DSI to estimate average distance
		* Use estimateAverageShift() instead - much more accurate. This one underestimates
		* up to twice (not understood)
		*
		* @param xyzatr0 - first scene [[x,y,z],[a,t,r]]
		* @param xyzatr1 - second scene [[x,y,z],[a,t,r]]
		* @param use_lma
		* @return
		*/
		@Deprecated
		public double estimatePixelShift(
		double [][] xyzatr0,
		double [][] xyzatr1,
		boolean use_lma
		) {
		return estimatePixelShift(
		xyzatr0,
		xyzatr1,
		use_lma,
		0);
		}
		public double estimatePixelShift(
		double [][] xyzatr0,
		double [][] xyzatr1,
		boolean use_lma,
		int mode
		) {
		boolean sign_x = (mode & 1) != 0;
		boolean sign_y = (mode & 2) != 0;
		double z = getAverageZ(use_lma);
		if (Double.isNaN(z)) {
		System.out.println("estimatePixelShift(): failed estimating distance - no DSI or too few DSI tiles.");
		@@ -452,8 +612,8 @@ public class QuadCLTCPU {
		for (int i = 0; i<2;i++) for (int j=0; j < 3; j++) {
		dxyzatr[i][j] = xyzatr1[i][j]-xyzatr0[i][j];
		}
		double eff_az = dxyzatr[1][0] + (sign_x?-1:1) * dxyzatr[0][0] / z;
		double eff_tl = dxyzatr[1][1] + (sign_y?-1:1) * dxyzatr[0][1] / z;
		double eff_az = dxyzatr[1][0] + dxyzatr[0][0] / z;
		double eff_tl = dxyzatr[1][1] + dxyzatr[0][1] / z;
		double pix_x = -eff_az*aztl_pix_per_rad;
		double pix_y = eff_tl*aztl_pix_per_rad;
		double pix_roll = dxyzatr[1][2] * roll_pix_per_rad;
		@@ -462,6 +622,20 @@ public class QuadCLTCPU {
		return est_pix;
		}

		/**
		* Estimate average pixel offset between 2 scenes for avoiding FPN. This scene
		* should have DSI to estimate average distance.
		* Difference with rectilinear is small so both are OK.
		* @param xyzatr0 first scene pose (double[3][2]) for reference scene
		* @param xyzatr1 second scene pose
		* @param average_z average distance (altitude AGL for downward images) in meters
		* @param use_rot when false - compare only the center, when true - average 4 samples
		* at 25% of width/height. May be needed for pure rotation (no center
		* shift), but may fail if interscene shift exceeds 25%. So try first
		* for center and use 4 samples only if center does not move.
		* @param rectilinear Ignore lens distortions
		* @return interscene shift in pixels
		*/
		public double estimateAverageShift(
		double [][] xyzatr0,
		double [][] xyzatr1,