Added vertical averaging (e2a311fc) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java

+15 −1

Original line number	Original line	Diff line number	Diff line
	@@ -118,6 +118,7 @@ import com.elphel.imagej.gpu.JCuda_ImageJ_Example_Plugin;
	import com.elphel.imagej.ims.DjiSrt;		import com.elphel.imagej.ims.DjiSrt;
	import com.elphel.imagej.ims.DjiSrtReader;		import com.elphel.imagej.ims.DjiSrtReader;
	import com.elphel.imagej.ims.EventLogger;		import com.elphel.imagej.ims.EventLogger;
			import com.elphel.imagej.ims.QuatVertLMA;
	import com.elphel.imagej.ims.UasLogReader;		import com.elphel.imagej.ims.UasLogReader;
	//import com.elphel.imagej.ims.Imx5;		//import com.elphel.imagej.ims.Imx5;
	import com.elphel.imagej.jp4.JP46_Reader_camera;		import com.elphel.imagej.jp4.JP46_Reader_camera;
	@@ -6164,11 +6165,24 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
	System.out.println("Failed to create eventLogger instance.");		System.out.println("Failed to create eventLogger instance.");
	return;		return;
	}		}
			double [][] quats_all = eventLogger.getQuats(
			CLT_PARAMETERS.imp.imsv_ts0, // 1763232236.0, // double ts_start, or NaN
			CLT_PARAMETERS.imp.imsv_ts1); // 1763234000.0); // double ts_end)
			double [] vert_xyz = null;
			if (quats_all != null) {
			vert_xyz = QuatVertLMA.getVertAndRms(
			CLT_PARAMETERS, // CLTParameters clt_parameters,
			quats_all, // double [][] quats,
			DEBUG_LEVEL); // int debugLevel)
			}

	eventLogger.print2Pps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"rec_1pps");		eventLogger.print2Pps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"rec_1pps");
	eventLogger.printStrobeInTime(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_strobe_in_time");		eventLogger.printStrobeInTime(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_strobe_in_time");
	eventLogger.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");		eventLogger.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");
	eventLogger.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);		eventLogger.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);
	eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1");		eventLogger.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1",
			vert_xyz); // may be null. 4-element - ignore last (it is RMS)

	eventLogger.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");		eventLogger.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");
	/*		/*
	if (EVENT_LOGGER == null) {		if (EVENT_LOGGER == null) {

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

+56 −2

Original line number	Original line	Diff line number	Diff line
	@@ -87,7 +87,7 @@ public class EventLogger {

	}		}

	public void printDidIns1(String out_did1_path) {		public void printDidIns1(String out_did1_path, double [] vert_xyz) {
	out_did1_path = Paths.get(out_did1_path).normalize().toString();		out_did1_path = Paths.get(out_did1_path).normalize().toString();
	Path op_did1 = Paths.get(out_did1_path);		Path op_did1 = Paths.get(out_did1_path);
	op_did1=op_did1.normalize();		op_did1=op_did1.normalize();
	@@ -97,7 +97,7 @@ public class EventLogger {
	if (logger_files[nf].hasIns(false)) {		if (logger_files[nf].hasIns(false)) {
	System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);		System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
	try {		try {
	logger_files[nf].listDidIns1(out_path,false);		logger_files[nf].listDidIns1(out_path,false, vert_xyz);
	} catch (IOException e) {		} catch (IOException e) {
	// TODO Auto-generated catch block		// TODO Auto-generated catch block
	e.printStackTrace();		e.printStackTrace();
	@@ -131,6 +131,60 @@ public class EventLogger {
	}		}
	}		}

			public double [][] getQuats(
			double ts_start,
			double ts_end){
			if (ts_start < 0) ts_start=Double.NaN;
			if (ts_end < 0) ts_end=Double.NaN;
			double [][] quats_all = null;
			double [][] quats = null;
			for (int nf = 0; nf < logger_files.length; nf++) {
			quats = null;
			if (logger_files[nf].hasIns(true)) {
			System.out.println("Processing all DID_INS_2 data in "+logger_files[nf].abs_path);
			try {
			quats = logger_files[nf].getQuats(
			ts_start, // double ts_start, // or NaN
			ts_end, // double ts_end, // or NaN
			false); // boolean keep) throws IOException {

			} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			}
			} else {
			System.out.println("DID_INS_2 data is not available in "+logger_files[nf].abs_path);
			}
			if ((quats == null) \|\| (quats.length ==0)) {
			System.out.println("Processing all DID_INS_1 data in "+logger_files[nf].abs_path);
			try {
			quats = logger_files[nf].getQuatsIns1(
			ts_start, // double ts_start, // or NaN
			ts_end, // double ts_end, // or NaN
			false); // boolean keep) throws IOException {

			} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			}
			}
			if ((quats != null) && (quats.length > 0)) {
			if (quats_all == null) {
			quats_all = quats;
			} else {
			double [][] quats0 = quats_all;
			quats_all = new double[quats0.length + quats.length][];
			System.arraycopy(quats0, 0, quats_all, 0, quats0.length);
			System.arraycopy(quats, 0, quats_all, quats0.length, quats.length);
			}
			}
			}
			return quats_all;
			}




	public void printPimu(String out_pimu_path) {		public void printPimu(String out_pimu_path) {
	out_pimu_path = Paths.get(out_pimu_path).normalize().toString();		out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
	Path op_pimu = Paths.get(out_pimu_path);		Path op_pimu = Paths.get(out_pimu_path);

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

+142 −3

Original line number	Original line	Diff line number	Diff line
	@@ -751,7 +751,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
	}		}


	public void listDidIns1(String path, boolean keep) throws IOException {		public void listDidIns1(String path, boolean keep, double [] vert_xyz) throws IOException {
	double max_offset = 1000; // m		double max_offset = 1000; // m
	open();		open();
	setInsQuats();		setInsQuats();
	@@ -772,7 +772,11 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {

	"u\tv\tw\t"+		"u\tv\tw\t"+
	"lat\tlong\talt\t"+		"lat\tlong\talt\t"+
	"ned:N\tned:E\tned:D\n";		"ned:N\tned:E\tned:D";
			if (vert_xyz != null) {
			header +="\tvert:X\tvert:Y\tvert:Z";
			}
			header +="\n";
	if (writer != null) {		if (writer != null) {
	writer.write(header);		writer.write(header);
	} else {		} else {
	@@ -813,7 +817,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
	String line =String.format(		String line =String.format(
	"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+		"%6d\t%17.6f\t%17.6f\t%10.3f\t%08x\t%08x\t%8.4f\t%8.4f\t%8.4f\t"+
	"%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+		"%8.4f\t%8.4f\t%8.4f\t%8.4f\t"+
	"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f\n",		"%8.3f\t%8.3f\t%8.3f\t%12.7f\t%12.7f\t%12.7f\t%8.3f\t%8.3f\t%8.3f",
	nrec,		nrec,
	getMasterTS(nrec),		getMasterTS(nrec),
	getLocalTS(nrec),		getLocalTS(nrec),
	@@ -826,6 +830,18 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
	did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2], // WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)		did_ins_1.lla[0], did_ins_1.lla[1], did_ins_1.lla[2], // WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
	did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2] // North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude		did_ins_1.ned[0] - start_ned[0], did_ins_1.ned[1] - start_ned[1], did_ins_1.ned[2] - start_ned[2] // North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude
	);		);
			if (vert_xyz != null) {
			double [] quat = {ins_quats[nrec][0],ins_quats[nrec][1],ins_quats[nrec][2],ins_quats[nrec][3]};
			double [][] R = QuatVertLMA.quaternionToRotationMatrix(quat);
			double [] ims_down = new double[3];
			for (int i = 0; i < 3; i++) {
			for (int k = 0; k < 3; k++) {
			ims_down[i] += R[i][k]*vert_xyz[k];
			}
			}
			line +=String.format("\t%13.10f\t%13.10f\t%13.10f",ims_down[0],ims_down[1],ims_down[2]);
			}
			line += "\n";
	if (writer != null) {		if (writer != null) {
	writer.write(line);		writer.write(line);
	} else {		} else {
	@@ -911,6 +927,129 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
	}		}
	}		}


			public double [][] getQuats(
			double ts_start, // or NaN
			double ts_end, // or NaN
			boolean keep) throws IOException {
			class Double4 {
			double [] quat = new double[4];
			Double4 (float [] fdata){
			double [] data = new double[fdata.length];
			for (int i = 0; i < data.length; i++) {
			data[i] = fdata[i];
			}
			}
			}
			ArrayList<Double4> quat_list = new ArrayList<Double4>();
			open();
			int dbg_nrec = -5000;
			int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) \| EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
			for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
			System.out.println("listDidIns2(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
			byte [] payload = getDidPayload(
			null, // next_fileinfo, // may be null if payload does not extend beyond single record
			nrec);
			if (payload == null) {
			System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
			break;
			}
			ByteBuffer bb_payload = ByteBuffer.wrap(payload);
			bb_payload.order(ByteOrder.LITTLE_ENDIAN);
			Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
			if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
			continue;
			}
			double ts = getMasterTS(nrec);
			if (!(ts < ts_start) && !(ts > ts_end)) {
			quat_list.add(new Double4(did_ins_2.qn2b));
			}
			}
			}
			if (!keep) {
			close();
			}
			double [][] quats = new double[quat_list.size()][];
			for (int i = 0; i < quats.length; i++) {
			quats[i] = quat_list.get(i).quat;
			}
			return quats;
			}



			public double [][] getQuatsIns1(
			double ts_start, // or NaN
			double ts_end, // or NaN
			boolean keep) throws IOException {
			class Double4 {
			double [] quat;
			Double4 (float [] fdata){
			quat = new double[fdata.length];
			for (int i = 0; i < quat.length; i++) {
			quat[i] = fdata[i];
			}
			return;
			}
			}
			ArrayList<Double4> quat_list = new ArrayList<Double4>();
			double max_offset = 1000; // m
			open();
			setInsQuats();
			float [] start_ned = null;
			int dbg_nrec = -5000;
			int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) \| EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
			for (int nrec = 0; nrec < num_recs; nrec++) {
			if (nrec == dbg_nrec) {
			System.out.println("getQuatsIns1(): nrec="+nrec);
			}
			int [] full_type = getFullType(bb, nrec);
			if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
			byte [] payload = getDidPayload(
			null, // next_fileinfo, // may be null if payload does not extend beyond single record
			nrec);
			if (payload == null) {
			System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
			break;
			}
			ByteBuffer bb_payload = ByteBuffer.wrap(payload);
			bb_payload.order(ByteOrder.LITTLE_ENDIAN);
			Did_ins_1 did_ins_1 = new Did_ins_1(bb_payload);
			if ((did_ins_1.uvw[0] == 0) && (did_ins_1.uvw[1] == 0) && (did_ins_1.uvw[2] == 0)) {
			continue;
			}
			if ((start_ned != null) &&
			( (Math.abs(start_ned[0] - did_ins_1.ned[0]) > max_offset) \|\|
			(Math.abs(start_ned[1] - did_ins_1.ned[1]) > max_offset) \|\|
			(Math.abs(start_ned[2] - did_ins_1.ned[2]) > max_offset)
			)){
			start_ned = null;
			}
			if (start_ned == null) {
			start_ned = did_ins_1.ned.clone();
			}
			start_ned = new float[] {0,0,0};

			double ts = getMasterTS(nrec);
			if (!(ts < ts_start) && !(ts > ts_end)) {
			quat_list.add(new Double4(ins_quats[nrec]));
			}
			}
			}
			if (!keep) {
			close();
			}
			double [][] quats = new double[quat_list.size()][];
			for (int i = 0; i < quats.length; i++) {
			quats[i] = quat_list.get(i).quat;
			}
			return quats;
			}

	public void listGPS (String path, int gps_mask, boolean keep) throws IOException {		public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
	// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS		// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
	// double max_offset = 1000; // m		// double max_offset = 1000; // m

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

0 → 100644

+787 −0

File added.

Preview size limit exceeded, changes collapsed.

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

+15 −0

Original line number	Original line	Diff line number	Diff line
	@@ -7947,6 +7947,21 @@ public class Interscene {
	}		}

	// relative to the GPS/compass		// relative to the GPS/compass
			/**
			* Finding correspondence between scene XYZ (ers_reference.getSceneXYZ(ts)) and IMS axes (
			* Imx5.applyQuaternionTo(
			* Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
			* ims_mount_atr, // small misalignment, provided in clt_parameters
			* ims_ortho), // fixed 90-degree rotation
			* @param clt_parameters - configuration parameters
			* @param quadCLTs
			* @param ref_scene
			* @param earliest_scene
			* @param rms
			* @param enu_corr
			* @param debugLevel
			* @return
			*/
	public static double [] getQuaternionCorrection(		public static double [] getQuaternionCorrection(
	CLTParameters clt_parameters,		CLTParameters clt_parameters,
	QuadCLT [] quadCLTs,		QuadCLT [] quadCLTs,

Admin message