Added attitude correction from IMS to camera by images (db9a0992) · Commits · Elphel / imagej-elphel

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

+22 −1

Original line number	Diff line number	Diff line
		@@ -175,7 +175,7 @@ public class Imx5 {
		quat_enu_rot.getQ3()};
		}

		public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
		public static double [] applyQuaternionTo(double[]quat, double[] vector, boolean inverse) {
		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		double [] rslt = new double[3];
		if (inverse) {
		@@ -187,6 +187,27 @@ public class Imx5 {
		return rslt;
		}

		public static double [] applyQuaternionToQuaternion(
		double[] quat,
		double[] targ_quat,
		boolean inverse) {
		Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
		Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
		Rotation rslt_rot;
		if (inverse) {
		rslt_rot=ims_rot.applyInverseTo(targ_rot);

		} else {
		rslt_rot=ims_rot.applyTo(targ_rot);
		}
		return new double[] {
		rslt_rot.getQ0(),
		rslt_rot.getQ1(),
		rslt_rot.getQ2(),
		rslt_rot.getQ3()};
		}


		public static double [] quaternionImsToCam(
		double [] quat, // ims_to_ned
		double [] ims_atr, // -> mount_to_cam

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

+105 −12

Original line number	Diff line number	Diff line
		@@ -430,7 +430,7 @@ public class Interscene {
		d2.getQEnu(),
		ims_mount_atr,
		ims_ortho);
		double [] cam_xyz_enu = Imx5.applyQuternionTo(
		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
		@@ -3789,6 +3789,15 @@ public class Interscene {
		int num_processed = 0;
		double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
		double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
		// for testing QuaternionLma
		double [] rms = new double [2];
		double [] quatCorr = getQuaternionCorrection(
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT [] quadCLTs,
		ref_index, // int ref_index,
		earliest_scene, // int earliest_scene,
		rms); // double [] rms // null or double[2];


		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
		QuadCLT scene = quadCLTs[nscene];
		@@ -3817,6 +3826,7 @@ public class Interscene {
		String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
		"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
		"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"+
		"\tINS->X\tINS->Y\tINS->Z"+
		"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
		"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
		"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
		@@ -3904,13 +3914,13 @@ public class Interscene {
		//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
		double [] double_qn2b = d2.getQn2b();
		double [] double_uvw = d2.getUvw();
		double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
		double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
		double [] uvw_dir = Imx5.applyQuaternionTo(double_qn2b, double_uvw, false); // bad
		double [] uvw_inv = Imx5.applyQuaternionTo(double_qn2b, double_uvw, true); // good
		//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp]

		double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
		double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
		double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
		double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
		// String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
		// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";

		@@ -3925,8 +3935,8 @@ public class Interscene {
		ims_mount_atr, // new double[] {0, 0.13, 0},
		ims_ortho);

		double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
		double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
		double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
		double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
		double [] cam_xyz_ned = test_xyz_ned(
		Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
		d2_ref.getQn2b(), // double[] quat_ned,
		@@ -3939,19 +3949,20 @@ public class Interscene {
		ims_mount_atr, // double [] ims_mount_atr,
		ims_ortho); //double [] ims_ortho)
		*/
		double [] cam_xyz_enu = Imx5.applyQuternionTo(
		double [] cam_xyz_enu = Imx5.applyQuaternionTo(
		Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
		ims_mount_atr,
		ims_ortho),
		enu,
		false);

		double [] quat_lma_enu_xyz = Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false);

		sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
		sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //

		sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); //
		sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); //
		sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); //

		double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);

		@@ -3993,6 +4004,10 @@ public class Interscene {
		// add lpf
		sb.append("\n");
		}
		// test QuaternionLMA here




		if (path!=null) {
		String footer=(comment != null) ? ("Comment: "+comment): "";
		@@ -4004,6 +4019,84 @@ public class Interscene {
		}
		}

		public static double [] getQuaternionCorrection(
		CLTParameters clt_parameters,
		QuadCLT [] quadCLTs,
		int ref_index,
		int earliest_scene,
		double [] rms // null or double[2];
		) {
		double [] ims_ortho = clt_parameters.imp.ims_ortho;
		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];
		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];
		} else {
		String ts = scene.getImageName();
		quat_lma_xyz[nscene] = 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(
		Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
		ims_mount_atr,
		ims_ortho),
		enu,
		false);
		}
		double lambda = 0.1;
		double lambda_scale_good = 0.5;
		double lambda_scale_bad = 8.0;
		double lambda_max = 100;
		double rms_diff = 0.001;
		int num_iter = 20;
		boolean last_run = false;
		double reg_w = 0.25;
		double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		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,
		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,
		debug_level); // 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
		lambda_scale_bad, // double lambda_scale_bad, // 8.0
		lambda_max, // double lambda_max, // 100
		rms_diff, // double rms_diff, // 0.001
		num_iter, // int num_iter, // 20
		last_run, // boolean last_run,
		debug_level); // int debug_level)
		if (lma_result < 0) {
		return null;
		} else {
		if (rms != null) { // null or double[2];
		double [] last_rms = quaternionLma.getLastRms();
		rms[0] = last_rms[0];
		rms[1] = last_rms[1];
		if (rms.length >= 4) {
		double [] initial_rms = quaternionLma.getInitialRms();
		rms[2] = initial_rms[0];
		rms[3] = initial_rms[1];
		if (rms.length >= 5) {
		rms[4] = lma_result;
		}
		}
		}
		return quaternionLma.getQuaternion();
		}
		}

		static double [] test_xyz_ned(
		double [] ned,
		double[] quat_ned,
		@@ -4013,7 +4106,7 @@ public class Interscene {
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_ned,
		ims_mount_atr,
		ims_ortho);
		return Imx5.applyQuternionTo(cam_quat2, ned, false);
		return Imx5.applyQuaternionTo(cam_quat2, ned, false);
		}

		static double [] test_xyz_enu(
		@@ -4025,7 +4118,7 @@ public class Interscene {
		double [] cam_quat2 =Imx5.quaternionImsToCam(quat_enu,
		ims_mount_atr,
		ims_ortho);
		return Imx5.applyQuternionTo(cam_quat2, enu, false);
		return Imx5.applyQuaternionTo(cam_quat2, enu, false);
		}

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

+24 −0

Original line number	Diff line number	Diff line
		/**
		**
		** IntersceneLma - Adjust camera egomotion with LMA
		**
		** Copyright (C) 2020-2023 Elphel, Inc.
		**
		-----------------------------------------------------------------------------
		**
		** IntersceneLma.java is free software: you can redistribute it and/or modify
		** it under the terms of the GNU General Public License as published by
		** the Free Software Foundation, either version 3 of the License, or
		** (at your option) any later version.
		**
		** This program is distributed in the hope that it will be useful,
		** but WITHOUT ANY WARRANTY; without even the implied warranty of
		** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		** GNU General Public License for more details.
		**
		** You should have received a copy of the GNU General Public License
		** along with this program. If not, see <http://www.gnu.org/licenses/>.
		-----------------------------------------------------------------------------
		**
		*/

		package com.elphel.imagej.tileprocessor;

		import java.io.ByteArrayOutputStream;

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

+15 −0

Original line number	Diff line number	Diff line
		@@ -85,6 +85,9 @@ public class IntersceneMatchParameters {
		public int min_num_orient = 2; // make from parameters, should be >= 1
		public int min_num_interscene = 2; // make from parameters, should be >= 1

		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 generate_egomotion = true; // generate egomotion table (image-based and ims)
		public boolean generate_mapped = true; // generate any of the sequences - Tiff, video, mono or stereo
		public boolean reuse_video = false; // dry-run video generation, just reassemble video fragments (active if !generate_mapped)
		@@ -536,6 +539,10 @@ public class IntersceneMatchParameters {
		"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
		gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
		"Minimal required number of re-calculations of the interscene-accumulated DSI.");
		gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
		"Calculate camera orientation correction from predicted by IMS .");
		gd.addCheckbox ("Apply IMS orientation correction", this.apply_quat_corr,
		"Apply camera orientation correction from predicted by IMS.");

		gd.addMessage ("Generate/show scene sequences");
		gd.addCheckbox ("Generate egomotion table", this.generate_egomotion,
		@@ -1227,6 +1234,8 @@ public class IntersceneMatchParameters {
		this.run_ly = gd.getNextBoolean();
		this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
		this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
		this.calc_quat_corr = gd.getNextBoolean();
		this.apply_quat_corr = gd.getNextBoolean();
		this.generate_egomotion = gd.getNextBoolean();
		this.generate_mapped = gd.getNextBoolean();
		this.reuse_video = gd.getNextBoolean();
		@@ -1609,6 +1618,8 @@ public class IntersceneMatchParameters {
		properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean
		properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int
		properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
		properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
		properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
		properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean
		properties.setProperty(prefix+"generate_mapped", this.generate_mapped+""); // boolean
		properties.setProperty(prefix+"reuse_video", this.reuse_video+""); // boolean
		@@ -1951,6 +1962,8 @@ public class IntersceneMatchParameters {
		if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));
		if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
		if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
		if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
		if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
		if (properties.getProperty(prefix+"generate_egomotion")!=null) this.generate_egomotion=Boolean.parseBoolean(properties.getProperty(prefix+"generate_egomotion"));
		if (properties.getProperty(prefix+"generate_mapped")!=null) this.generate_mapped=Boolean.parseBoolean(properties.getProperty(prefix+"generate_mapped"));
		if (properties.getProperty(prefix+"reuse_video")!=null) this.reuse_video=Boolean.parseBoolean(properties.getProperty(prefix+"reuse_video"));
		@@ -2320,6 +2333,8 @@ public class IntersceneMatchParameters {
		imp.run_ly = this.run_ly;
		imp.min_num_orient = this.min_num_orient;
		imp.min_num_interscene = this.min_num_interscene;
		imp.calc_quat_corr = this.calc_quat_corr;
		imp.apply_quat_corr = this.apply_quat_corr;
		imp.generate_egomotion = this.generate_egomotion;
		imp.generate_mapped = this.generate_mapped;
		imp.reuse_video = this.reuse_video;

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

+27 −2

Original line number	Diff line number	Diff line
		@@ -4655,8 +4655,9 @@ public class OpticalFlow {
		save_mapped_mono_color[1] \|\| gen_avi_mono_color[1] \|\| show_mono_color[1]} ;
		// skip completely if no color or mono, tiff or video

		boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
		boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
		boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)

		boolean generate_mapped = clt_parameters.imp.generate_mapped &&
		(gen_seq_mono_color[0] \|\| gen_seq_mono_color[1]); // generate sequences - Tiff and/or video
		boolean export3d = clt_parameters.imp.export3d; // true;
		@@ -5329,6 +5330,30 @@ public class OpticalFlow {
		}
		}

		if (calc_quat_corr) {
		double [] quat_rms = new double [5];
		double [] quatCorr = Interscene.getQuaternionCorrection(
		clt_parameters, // CLTParameters clt_parameters,
		quadCLTs, // QuadCLT [] quadCLTs,
		ref_index, // int ref_index,
		earliest_scene, // int earliest_scene,
		quat_rms); // double [] rms // null or double[2];
		if (quatCorr != null) {
		int num_iter = (int) quat_rms[4];
		if (debugLevel> -3) {
		System.out.println("LMA done on iteration "+num_iter+
		" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
		System.out.println("Correction quaternion = ["+quatCorr[0]+", "+
		quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
		}
		quadCLTs[ref_index].setQuatCorr(quatCorr);
		quadCLT_main.setQuatCorr(quatCorr);
		quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
		null, // String path, // full name with extension or w/o path to use x3d directory
		debugLevel+1);
		}
		}

		if (generate_egomotion) {
		boolean ego_show = !clt_parameters.batch_run; //true;
		String ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+

Admin message