Before interscene refactoring (21b8905c) · Commits · Elphel / imagej-elphel

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

+32 −1

Original line number	Original line	Diff line number	Diff line
	@@ -10,6 +10,8 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
	import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;		import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
	import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;		import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;

			import com.elphel.imagej.tileprocessor.ErsCorrection;


	public class Imx5 {		public class Imx5 {

	@@ -173,6 +175,35 @@ public class Imx5 {
	return rslt;		return rslt;
	}		}

			public static double [] quaternionImsToCam(double[]quat) {
			double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
			Rotation cam_to_ims0 = new Rotation(ort, 1E-8);
			Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
			Rotation cam_quat = cam_to_ims0.applyTo(ims_rot);
			return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
			}

			/**
			* Get quaternion from IMS data, fixed ortho rotation for IMU to camera and small
			* additional correction of IMU mount to the camera using camera axes (Y- up, X - right,
			* z - back from the target)
			* @param quat IMU-measured quaternion
			* @param ims_atr azimuth, tilt, roll of the IMU mount relative to the camera axes
			* @return quaternion to apply to NED to get camera-referenced coordinates
			*/
			public static double [] quaternionImsToCam(double[]quat, double [] ims_atr) {
			double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
			Rotation ims_to_mount_ortho = new Rotation(ort, 1E-8);
			Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
			Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
			ims_atr[0], ims_atr[1], ims_atr[2]);
			Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
			Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
			return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
			}



	public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {		public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
	RotationConvention rc = RotationConvention.FRAME_TRANSFORM;		RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
	// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;		// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
	@@ -226,8 +257,8 @@ public class Imx5 {

	public static double [] nedFromLla (double [] lla, double [] lla_ref) {		public static double [] nedFromLla (double [] lla, double [] lla_ref) {
	double [] ned = new double[] {		double [] ned = new double[] {
	EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)(lla[1] - lla_ref[1]) Math.PI / 180.0,
	EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,		EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
			EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)(lla[1] - lla_ref[1]) Math.PI / 180.0,
	-(lla[2] - lla_ref[2])		-(lla[2] - lla_ref[2])
	};		};
	return ned;		return ned;

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

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -187,7 +187,7 @@ public class ErsCorrection extends GeometryCorrection {
	DP_DVX, DP_DVY, DP_DVZ,		DP_DVX, DP_DVY, DP_DVZ,
	DP_DSVAZ, DP_DSVTL, DP_DSVRL,		DP_DSVAZ, DP_DSVTL, DP_DSVRL,
	DP_DSVX, DP_DSVY, DP_DSVZ};		DP_DSVX, DP_DSVY, DP_DSVZ};
	static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;		public static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
	static final double THRESHOLD = 1E-10;		static final double THRESHOLD = 1E-10;
	static final double LINE_ERR = 0.001; // line accuracy for ERS when converting from world to pixels.		static final double LINE_ERR = 0.001; // line accuracy for ERS when converting from world to pixels.
	// parameters for the ERS distortion calculation		// parameters for the ERS distortion calculation

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

+31 −14

Original line number	Original line	Diff line number	Diff line
	@@ -26,12 +26,8 @@ package com.elphel.imagej.tileprocessor;
	import java.awt.Color;		import java.awt.Color;
	import java.awt.Font;		import java.awt.Font;
	import java.awt.Rectangle;		import java.awt.Rectangle;
	import java.io.ByteArrayOutputStream;
	import java.io.File;		import java.io.File;
	import java.io.IOException;		import java.io.IOException;
	import java.io.ObjectOutputStream;
	import java.io.Serializable;
	import java.security.MessageDigest;
	import java.security.NoSuchAlgorithmException;		import java.security.NoSuchAlgorithmException;
	import java.util.ArrayList;		import java.util.ArrayList;
	import java.util.Arrays;		import java.util.Arrays;
	@@ -42,9 +38,6 @@ import java.util.Properties;
	import java.util.concurrent.atomic.AtomicBoolean;		import java.util.concurrent.atomic.AtomicBoolean;
	import java.util.concurrent.atomic.AtomicInteger;		import java.util.concurrent.atomic.AtomicInteger;
	import java.util.concurrent.atomic.DoubleAccumulator;		import java.util.concurrent.atomic.DoubleAccumulator;
	import javax.xml.bind.DatatypeConverter;

	import org.apache.commons.math3.complex.Quaternion;

	import java.util.concurrent.ThreadLocalRandom;		import java.util.concurrent.ThreadLocalRandom;

	@@ -55,14 +48,11 @@ import com.elphel.imagej.cameras.EyesisCorrectionParameters;
	import com.elphel.imagej.common.DoubleGaussianBlur;		import com.elphel.imagej.common.DoubleGaussianBlur;
	import com.elphel.imagej.common.ShowDoubleFloatArrays;		import com.elphel.imagej.common.ShowDoubleFloatArrays;
	import com.elphel.imagej.correction.CorrectionColorProc;		import com.elphel.imagej.correction.CorrectionColorProc;
	import com.elphel.imagej.correction.Eyesis_Correction;
	import com.elphel.imagej.gpu.GpuQuad;		import com.elphel.imagej.gpu.GpuQuad;
	import com.elphel.imagej.gpu.TpTask;		import com.elphel.imagej.gpu.TpTask;
	import com.elphel.imagej.ims.Did_gps_pos;
	import com.elphel.imagej.ims.Did_ins_1;		import com.elphel.imagej.ims.Did_ins_1;
	import com.elphel.imagej.ims.Did_ins_2;		import com.elphel.imagej.ims.Did_ins_2;
	import com.elphel.imagej.ims.Did_pimu;		import com.elphel.imagej.ims.Did_pimu;
	import com.elphel.imagej.ims.EventLogger;
	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;

	@@ -76,7 +66,6 @@ import ij.process.FloatProcessor;
	import ij.process.ImageConverter;		import ij.process.ImageConverter;
	import ij.process.ImageProcessor;		import ij.process.ImageProcessor;
	import ij.text.TextWindow;		import ij.text.TextWindow;
	import ij.plugin.filter.AVI_Writer;
	import ij.plugin.filter.GaussianBlur;		import ij.plugin.filter.GaussianBlur;

	public class OpticalFlow {		public class OpticalFlow {
	@@ -134,7 +123,7 @@ public class OpticalFlow {
	{W,W,1,W,W,W,1,W},		{W,W,1,W,W,W,1,W},
	{W,W,W,1,W,W,W,1},		{W,W,W,1,W,W,W,1},
	{W,1,W,W,W,1,W,W}};		{W,1,W,W,W,1,W,W}};
	// Same for a larged kernel (5x5)		// Same for a larger kernel (5x5)
	public static int [][] KERN_FG2 = {		public static int [][] KERN_FG2 = {
	// 0 1 2 3 4 5 6 7 8 91011121314151617181920212223		// 0 1 2 3 4 5 6 7 8 91011121314151617181920212223
	{1,1,3,2,2,2,3,1,1,1,0,0,3,0,0,2,2,2,0,0,3,0,0,1},		{1,1,3,2,2,2,3,1,1,1,0,0,3,0,0,2,2,2,0,0,3,0,0,1},
	@@ -6251,7 +6240,9 @@ public class OpticalFlow {
	"\tlat\tlong\talt\tned0\tned1\tned2";		"\tlat\tlong\talt\tned0\tned1\tned2";
	String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";		String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
	String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+		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"+
	"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";		"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";

	String header_pimu="\to0\to1\to2\ta0\ta1\ta2";		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";

	String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";		String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
	@@ -6377,7 +6368,7 @@ public class OpticalFlow {
	sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);		sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);
	sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);		sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);
	sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);		sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);
	// String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlong\tlat\talt";		//String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
	sb.append("\t"+d2.timeOfWeek);		sb.append("\t"+d2.timeOfWeek);
	sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);		sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
	sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);		sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
	@@ -6392,14 +6383,40 @@ public class OpticalFlow {

	double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);		double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
	double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);		double [] ims_xyz = Imx5.applyQuternionTo(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";

	sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes		sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
	sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes		sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes

			// double [] quaternionImsToCam(double[]quat, boolean rvrs)
			double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b);
			double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
			// new double[] {Math.PI/4,0,0});
			new double[] {0, 0.13, 0});
			double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
			double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);

			/*
			public static double [] quaternionImsToCam(double[]quat, double [] ims_atr) {
			double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
			Rotation ims_to_mount_ortho = new Rotation(ort, 1E-8);
			Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
			Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
			ims_atr[0], ims_atr[1], ims_atr[2]);
			Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
			Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
			return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
			}

			*/

			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"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong		sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
	sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct		sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct


	// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";		// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
	sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));		sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
	sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));		sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));

Admin message