Commit 21b8905c authored by Andrey Filippov's avatar Andrey Filippov

Before interscene refactoring

parent b9a32b13
...@@ -10,6 +10,8 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation; ...@@ -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 { ...@@ -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 { ...@@ -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;
......
...@@ -187,7 +187,7 @@ public class ErsCorrection extends GeometryCorrection { ...@@ -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
......
...@@ -26,12 +26,8 @@ package com.elphel.imagej.tileprocessor; ...@@ -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; ...@@ -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; ...@@ -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; ...@@ -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 { ...@@ -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 { ...@@ -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 { ...@@ -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 { ...@@ -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));
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment