Skip to content

I
imagej-elphel
Commit d693f718 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

added East-North-Up that better matches camera orientation. Tested

parent 4d5b60b2
Show whitespace changes
Inline Side-by-side
Showing with 753 additions and 342 deletions
src/main/java/com/elphel/imagej/ims/Did_ins_1.java
View file @ d693f718
......@@ -27,6 +27,16 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
/** North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude */
public float [] ned = new float [3];
public double [] getTheta() {
return new double [] {theta[0], theta[1], theta[2]};
}
public double [] getUvw() {
return new double [] {uvw[0], uvw[1], uvw[2]};
}
public double [] getNed() {
return new double [] {ned[0], ned[1], ned[2]};
}
public Did_ins_1 (ByteBuffer bb) {
bb.order(ByteOrder.LITTLE_ENDIAN);
week= bb.getInt();
......
src/main/java/com/elphel/imagej/ims/Did_ins_2.java
View file @ d693f718
......@@ -5,6 +5,8 @@ import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
//public class Did_ins_1 implements Serializable {
......@@ -39,7 +41,25 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
public Did_ins_2(String prefix, Properties properties) {
getProperties(prefix, properties);
}
public double [] getQn2b() {
return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
}
public double [] getQEnu () {
Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5), Math.sqrt(0.5), 0, true);
Rotation quat_rot = new Rotation(qn2b[0],qn2b[1],qn2b[2],qn2b[3],true);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
return new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
quat_enu_rot.getQ2(),
quat_enu_rot.getQ3()};
}
public double [] getUvw() {
return new double [] {uvw[0], uvw[1], uvw[2]};
}
public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
Did_ins_2 new_did = new Did_ins_2();
......
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ d693f718
......@@ -163,6 +163,18 @@ public class Imx5 {
// static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
// RotationConvention.VECTOR_OPERATOR
//RotationOrder.YXZ, ROT_CONV
// move to did?
public static double [] quatEnu (double [] quat_ned) {
Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5), Math.sqrt(0.5), 0, true);
Rotation quat_rot = new Rotation(quat_ned[0],quat_ned[1],quat_ned[2],quat_ned[3],true);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
return new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
quat_enu_rot.getQ2(),
quat_enu_rot.getQ3()};
}
public static double [] applyQuternionTo(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];
......@@ -202,7 +214,23 @@ public class Imx5 {
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
}
public static double [] quaternionImsToCam(
double[]quat,
double [] ims_atr,
double [] quat_ort) {
Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
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 [] quatToCamAtr(double[]quat) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
}
public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
......@@ -264,4 +292,14 @@ public class Imx5 {
return ned;
}
public static double [] enuFromLla (double [] lla, double [] lla_ref) {
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,
(lla[2] - lla_ref[2])
};
return ned;
}
}
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ d693f718
......@@ -3,14 +3,22 @@ package com.elphel.imagej.tileprocessor;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.Did_ins_1;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.Imx5;
import ij.ImagePlus;
import ij.text.TextWindow;
public class Interscene {
public final OpticalFlow opticalFlow;
......@@ -3078,5 +3086,636 @@ public class Interscene {
return tp_tasks_ref;
}
public static void generateEgomotionTable0(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
String path,
String comment) {
boolean use_euler = true;
boolean use_qn2b = true;
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
// double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
"\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_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"+
"\tabs_A\tabs_T\tabs_R\trel_A\trel_T\trel_R"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
String header_cam001= "\ttheta0-001\ttheta1-001\ttheta2-001";
String header_cam010= "\ttheta0-010\ttheta1-010\ttheta2-010";
String header_cam011= "\ttheta0-011\ttheta1-011\ttheta2-011";
String header_cam100= "\ttheta0-100\ttheta1-100\ttheta2-100";
String header_cam101= "\ttheta0-101\ttheta1-101\ttheta2-101";
String header_cam110= "\ttheta0-110\ttheta1-110\ttheta2-110";
String header_cam111= "\ttheta0-111\ttheta1-111\ttheta2-111";
String header_cam200= "\ttheta0-200\ttheta1-200\ttheta2-200";
String header_cam201= "\ttheta0-201\ttheta1-201\ttheta2-201";
String header_cam210= "\ttheta0-210\ttheta1-210\ttheta2-210";
String header_cam211= "\ttheta0-211\ttheta1-211\ttheta2-211";
String header_cam300= "\ttheta0-300\ttheta1-300\ttheta2-300";
String header_cam301= "\ttheta0-301\ttheta1-301\ttheta2-301";
String header_cam310= "\ttheta0-310\ttheta1-310\ttheta2-310";
String header_cam311= "\ttheta0-311\ttheta1-311\ttheta2-311";
String header_cam400= "\ttheta0-400\ttheta1-400\ttheta2-400";
String header_cam401= "\ttheta0-401\ttheta1-401\ttheta2-401";
String header_cam410= "\ttheta0-410\ttheta1-410\ttheta2-410";
String header_cam411= "\ttheta0-411\ttheta1-411\ttheta2-411";
String header_cam500= "\ttheta0-500\ttheta1-500\ttheta2-500";
String header_cam501= "\ttheta0-501\ttheta1-501\ttheta2-501";
String header_cam510= "\ttheta0-510\ttheta1-510\ttheta2-510";
String header_cam511= "\ttheta0-511\ttheta1-511\ttheta2-511";
String header_qn2b000= "\tqn2b0-000\tqn2b1-000\tqn2b2-000";
String header_qn2b001= "\tqn2b0-001\tqn2b1-001\tqn2b2-001";
String header_qn2b010= "\tqn2b0-010\tqn2b1-010\tqn2b2-010";
String header_qn2b011= "\tqn2b0-011\tqn2b1-011\tqn2b2-011";
String header_qn2b100= "\tqn2b0-100\tqn2b1-100\tqn2b2-100";
String header_qn2b101= "\tqn2b0-101\tqn2b1-101\tqn2b2-101";
String header_qn2b110= "\tqn2b0-110\tqn2b1-110\tqn2b2-110";
String header_qn2b111= "\tqn2b0-111\tqn2b1-111\tqn2b2-111";
String header_qn2b200= "\tqn2b0-200\tqn2b1-200\tqn2b2-200";
String header_qn2b201= "\tqn2b0-201\tqn2b1-201\tqn2b2-201";
String header_qn2b210= "\tqn2b0-210\tqn2b1-210\tqn2b2-210";
String header_qn2b211= "\tqn2b0-211\tqn2b1-211\tqn2b2-211";
String header_qn2b300= "\tqn2b0-300\tqn2b1-300\tqn2b2-300";
String header_qn2b301= "\tqn2b0-301\tqn2b1-301\tqn2b2-301";
String header_qn2b310= "\tqn2b0-310\tqn2b1-310\tqn2b2-310";
String header_qn2b311= "\tqn2b0-311\tqn2b1-311\tqn2b2-311";
String header_qn2b400= "\tqn2b0-400\tqn2b1-400\tqn2b2-400";
String header_qn2b401= "\tqn2b0-401\tqn2b1-401\tqn2b2-401";
String header_qn2b410= "\tqn2b0-410\tqn2b1-410\tqn2b2-410";
String header_qn2b411= "\tqn2b0-411\tqn2b1-411\tqn2b2-411";
String header_qn2b500= "\tqn2b0-500\tqn2b1-500\tqn2b2-500";
String header_qn2b501= "\tqn2b0-501\tqn2b1-501\tqn2b2-501";
String header_qn2b510= "\tqn2b0-510\tqn2b1-510\tqn2b2-510";
String header_qn2b511= "\tqn2b0-511\tqn2b1-511\tqn2b2-511";
String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
if (use_qn2b) {
header+=
header_qn2b000 + header_qn2b001 + header_qn2b010 + header_qn2b011 +
header_qn2b100 + header_qn2b101 + header_qn2b110 + header_qn2b111 +
header_qn2b200 + header_qn2b201 + header_qn2b210 + header_qn2b211 +
header_qn2b300 + header_qn2b301 + header_qn2b310 + header_qn2b311 +
header_qn2b400 + header_qn2b401 + header_qn2b410 + header_qn2b411 +
header_qn2b500 + header_qn2b501 + header_qn2b510 + header_qn2b511;
}
if (use_euler) {
header+=
header_cam000 + header_cam001 + header_cam010 + header_cam011 +
header_cam100 + header_cam101 + header_cam110 + header_cam111 +
header_cam200 + header_cam201 + header_cam210 + header_cam211 +
header_cam300 + header_cam301 + header_cam310 + header_cam311 +
header_cam400 + header_cam401 + header_cam410 + header_cam411 +
header_cam500 + header_cam501 + header_cam510 + header_cam511;
}
StringBuffer sb = new StringBuffer();
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
scenes_xyzatr[nscene] = new double[2][3];
} else {
String ts = scene.getImageName();
scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
}
}
double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref);
double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (scene == null) {
for (int i = 0; i < 26; i++) {
sb.append("\t---");
}
sb.append("\n");
continue;
}
double timestamp = scene.getTimeStamp();
Did_ins_1 d1 = scene.did_ins_1;
Did_ins_2 d2 = scene.did_ins_2;
Did_pimu d3 = scene.did_pimu;
sb.append(nscene+"\t"+timestamp);
// TODO: try saved, not calculated velocities!
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
// String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
// "\tlong\tlat\talt\tned0\tned1\tned2";
sb.append("\t"+d1.timeOfWeek);
sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[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.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)\tlat\tlong\talt";
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.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.lla[0]+ "\t"+d2.lla[1]+ "\t"+d2.lla[2]);
double [] double_theta = d1.getTheta();
//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
//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 [] 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"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
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);
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]); //
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
double [] scene_rel_atr=ErsCorrection.combineXYZATR(
ims_scene_xyzatr,
ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
sb.append("\t"+scene_abs_atr[0]+ "\t"+scene_abs_atr[1]+ "\t"+scene_abs_atr[2]); //
sb.append("\t"+scene_rel_atr[0]+ "\t"+scene_rel_atr[1]+ "\t"+scene_rel_atr[2]); //
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
// 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.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
if (use_qn2b) {
double [] cam_qn2b_000 = Imx5.imsQToCamRotations(double_qn2b, 0, false, false);
double [] cam_qn2b_001 = Imx5.imsQToCamRotations(double_qn2b, 0, false, true);
double [] cam_qn2b_010 = Imx5.imsQToCamRotations(double_qn2b, 0, true, false);
double [] cam_qn2b_011 = Imx5.imsQToCamRotations(double_qn2b, 0, true, true);
double [] cam_qn2b_100 = Imx5.imsQToCamRotations(double_qn2b, 1, false, false);
double [] cam_qn2b_101 = Imx5.imsQToCamRotations(double_qn2b, 1, false, true);
double [] cam_qn2b_110 = Imx5.imsQToCamRotations(double_qn2b, 1, true, false);
double [] cam_qn2b_111 = Imx5.imsQToCamRotations(double_qn2b, 1, true, true);
double [] cam_qn2b_200 = Imx5.imsQToCamRotations(double_qn2b, 2, false, false);
double [] cam_qn2b_201 = Imx5.imsQToCamRotations(double_qn2b, 2, false, true);
double [] cam_qn2b_210 = Imx5.imsQToCamRotations(double_qn2b, 2, true, false);
double [] cam_qn2b_211 = Imx5.imsQToCamRotations(double_qn2b, 2, true, true);
double [] cam_qn2b_300 = Imx5.imsQToCamRotations(double_qn2b, 3, false, false);
double [] cam_qn2b_301 = Imx5.imsQToCamRotations(double_qn2b, 3, false, true);
double [] cam_qn2b_310 = Imx5.imsQToCamRotations(double_qn2b, 3, true, false);
double [] cam_qn2b_311 = Imx5.imsQToCamRotations(double_qn2b, 3, true, true);
double [] cam_qn2b_400 = Imx5.imsQToCamRotations(double_qn2b, 4, false, false);
double [] cam_qn2b_401 = Imx5.imsQToCamRotations(double_qn2b, 4, false, true);
double [] cam_qn2b_410 = Imx5.imsQToCamRotations(double_qn2b, 4, true, false);
double [] cam_qn2b_411 = Imx5.imsQToCamRotations(double_qn2b, 4, true, true);
double [] cam_qn2b_500 = Imx5.imsQToCamRotations(double_qn2b, 5, false, false);
double [] cam_qn2b_501 = Imx5.imsQToCamRotations(double_qn2b, 5, false, true);
double [] cam_qn2b_510 = Imx5.imsQToCamRotations(double_qn2b, 5, true, false);
double [] cam_qn2b_511 = Imx5.imsQToCamRotations(double_qn2b, 5, true, true);
sb.append("\t"+cam_qn2b_000[0]+"\t"+cam_qn2b_000[1]+"\t"+cam_qn2b_000[2]);
sb.append("\t"+cam_qn2b_001[0]+"\t"+cam_qn2b_001[1]+"\t"+cam_qn2b_001[2]);
sb.append("\t"+cam_qn2b_010[0]+"\t"+cam_qn2b_010[1]+"\t"+cam_qn2b_010[2]);
sb.append("\t"+cam_qn2b_011[0]+"\t"+cam_qn2b_011[1]+"\t"+cam_qn2b_011[2]);
sb.append("\t"+cam_qn2b_100[0]+"\t"+cam_qn2b_100[1]+"\t"+cam_qn2b_100[2]);
sb.append("\t"+cam_qn2b_101[0]+"\t"+cam_qn2b_101[1]+"\t"+cam_qn2b_101[2]);
sb.append("\t"+cam_qn2b_110[0]+"\t"+cam_qn2b_110[1]+"\t"+cam_qn2b_110[2]);
sb.append("\t"+cam_qn2b_111[0]+"\t"+cam_qn2b_111[1]+"\t"+cam_qn2b_111[2]);
sb.append("\t"+cam_qn2b_200[0]+"\t"+cam_qn2b_200[1]+"\t"+cam_qn2b_200[2]);
sb.append("\t"+cam_qn2b_201[0]+"\t"+cam_qn2b_201[1]+"\t"+cam_qn2b_201[2]);
sb.append("\t"+cam_qn2b_210[0]+"\t"+cam_qn2b_210[1]+"\t"+cam_qn2b_210[2]);
sb.append("\t"+cam_qn2b_211[0]+"\t"+cam_qn2b_211[1]+"\t"+cam_qn2b_211[2]);
sb.append("\t"+cam_qn2b_300[0]+"\t"+cam_qn2b_300[1]+"\t"+cam_qn2b_300[2]);
sb.append("\t"+cam_qn2b_301[0]+"\t"+cam_qn2b_301[1]+"\t"+cam_qn2b_301[2]);
sb.append("\t"+cam_qn2b_310[0]+"\t"+cam_qn2b_310[1]+"\t"+cam_qn2b_310[2]);
sb.append("\t"+cam_qn2b_311[0]+"\t"+cam_qn2b_311[1]+"\t"+cam_qn2b_311[2]);
sb.append("\t"+cam_qn2b_400[0]+"\t"+cam_qn2b_400[1]+"\t"+cam_qn2b_400[2]);
sb.append("\t"+cam_qn2b_401[0]+"\t"+cam_qn2b_401[1]+"\t"+cam_qn2b_401[2]);
sb.append("\t"+cam_qn2b_410[0]+"\t"+cam_qn2b_410[1]+"\t"+cam_qn2b_410[2]);
sb.append("\t"+cam_qn2b_411[0]+"\t"+cam_qn2b_411[1]+"\t"+cam_qn2b_411[2]);
sb.append("\t"+cam_qn2b_500[0]+"\t"+cam_qn2b_500[1]+"\t"+cam_qn2b_500[2]);
sb.append("\t"+cam_qn2b_501[0]+"\t"+cam_qn2b_501[1]+"\t"+cam_qn2b_501[2]);
sb.append("\t"+cam_qn2b_510[0]+"\t"+cam_qn2b_510[1]+"\t"+cam_qn2b_510[2]);
sb.append("\t"+cam_qn2b_511[0]+"\t"+cam_qn2b_511[1]+"\t"+cam_qn2b_511[2]);
}
if (use_euler) {
double [] cam_theta_000 = Imx5.imsToCamRotations(double_theta, 0, false, false);
double [] cam_theta_001 = Imx5.imsToCamRotations(double_theta, 0, false, true);
double [] cam_theta_010 = Imx5.imsToCamRotations(double_theta, 0, true, false);
double [] cam_theta_011 = Imx5.imsToCamRotations(double_theta, 0, true, true);
double [] cam_theta_100 = Imx5.imsToCamRotations(double_theta, 1, false, false);
double [] cam_theta_101 = Imx5.imsToCamRotations(double_theta, 1, false, true);
double [] cam_theta_110 = Imx5.imsToCamRotations(double_theta, 1, true, false);
double [] cam_theta_111 = Imx5.imsToCamRotations(double_theta, 1, true, true);
double [] cam_theta_200 = Imx5.imsToCamRotations(double_theta, 2, false, false);
double [] cam_theta_201 = Imx5.imsToCamRotations(double_theta, 2, false, true);
double [] cam_theta_210 = Imx5.imsToCamRotations(double_theta, 2, true, false);
double [] cam_theta_211 = Imx5.imsToCamRotations(double_theta, 2, true, true);
double [] cam_theta_300 = Imx5.imsToCamRotations(double_theta, 3, false, false);
double [] cam_theta_301 = Imx5.imsToCamRotations(double_theta, 3, false, true);
double [] cam_theta_310 = Imx5.imsToCamRotations(double_theta, 3, true, false);
double [] cam_theta_311 = Imx5.imsToCamRotations(double_theta, 3, true, true);
double [] cam_theta_400 = Imx5.imsToCamRotations(double_theta, 4, false, false);
double [] cam_theta_401 = Imx5.imsToCamRotations(double_theta, 4, false, true);
double [] cam_theta_410 = Imx5.imsToCamRotations(double_theta, 4, true, false);
double [] cam_theta_411 = Imx5.imsToCamRotations(double_theta, 4, true, true);
double [] cam_theta_500 = Imx5.imsToCamRotations(double_theta, 5, false, false);
double [] cam_theta_501 = Imx5.imsToCamRotations(double_theta, 5, false, true);
double [] cam_theta_510 = Imx5.imsToCamRotations(double_theta, 5, true, false);
double [] cam_theta_511 = Imx5.imsToCamRotations(double_theta, 5, true, true);
sb.append("\t"+cam_theta_000[0]+"\t"+cam_theta_000[1]+"\t"+cam_theta_000[2]);
sb.append("\t"+cam_theta_001[0]+"\t"+cam_theta_001[1]+"\t"+cam_theta_001[2]);
sb.append("\t"+cam_theta_010[0]+"\t"+cam_theta_010[1]+"\t"+cam_theta_010[2]);
sb.append("\t"+cam_theta_011[0]+"\t"+cam_theta_011[1]+"\t"+cam_theta_011[2]);
sb.append("\t"+cam_theta_100[0]+"\t"+cam_theta_100[1]+"\t"+cam_theta_100[2]);
sb.append("\t"+cam_theta_101[0]+"\t"+cam_theta_101[1]+"\t"+cam_theta_101[2]);
sb.append("\t"+cam_theta_110[0]+"\t"+cam_theta_110[1]+"\t"+cam_theta_110[2]);
sb.append("\t"+cam_theta_111[0]+"\t"+cam_theta_111[1]+"\t"+cam_theta_111[2]);
sb.append("\t"+cam_theta_200[0]+"\t"+cam_theta_200[1]+"\t"+cam_theta_200[2]);
sb.append("\t"+cam_theta_201[0]+"\t"+cam_theta_201[1]+"\t"+cam_theta_201[2]);
sb.append("\t"+cam_theta_210[0]+"\t"+cam_theta_210[1]+"\t"+cam_theta_210[2]);
sb.append("\t"+cam_theta_211[0]+"\t"+cam_theta_211[1]+"\t"+cam_theta_211[2]);
sb.append("\t"+cam_theta_300[0]+"\t"+cam_theta_300[1]+"\t"+cam_theta_300[2]);
sb.append("\t"+cam_theta_301[0]+"\t"+cam_theta_301[1]+"\t"+cam_theta_301[2]);
sb.append("\t"+cam_theta_310[0]+"\t"+cam_theta_310[1]+"\t"+cam_theta_310[2]);
sb.append("\t"+cam_theta_311[0]+"\t"+cam_theta_311[1]+"\t"+cam_theta_311[2]);
sb.append("\t"+cam_theta_400[0]+"\t"+cam_theta_400[1]+"\t"+cam_theta_400[2]);
sb.append("\t"+cam_theta_401[0]+"\t"+cam_theta_401[1]+"\t"+cam_theta_401[2]);
sb.append("\t"+cam_theta_410[0]+"\t"+cam_theta_410[1]+"\t"+cam_theta_410[2]);
sb.append("\t"+cam_theta_411[0]+"\t"+cam_theta_411[1]+"\t"+cam_theta_411[2]);
sb.append("\t"+cam_theta_500[0]+"\t"+cam_theta_500[1]+"\t"+cam_theta_500[2]);
sb.append("\t"+cam_theta_501[0]+"\t"+cam_theta_501[1]+"\t"+cam_theta_501[2]);
sb.append("\t"+cam_theta_510[0]+"\t"+cam_theta_510[1]+"\t"+cam_theta_510[2]);
sb.append("\t"+cam_theta_511[0]+"\t"+cam_theta_511[1]+"\t"+cam_theta_511[2]);
}
/*
sb.append("\t"+
cam_theta[0]*180/Math.PI+"\t"+
cam_theta[1]*180/Math.PI+"\t"+
cam_theta[2]*180/Math.PI);
*/
sb.append("\n");
}
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
CalibrationFileManagement.saveStringToFile (
path,
header+"\n"+sb.toString()+"\n"+footer);
}else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
public static void generateEgomotionTable(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
String path,
String comment) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
// double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
"\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_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->X2\tenu->Y\tenu->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";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
StringBuffer sb = new StringBuffer();
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
scenes_xyzatr[nscene] = new double[2][3];
} else {
String ts = scene.getImageName();
scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
}
}
double [][][] dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
double [] cam_quat_ref =Imx5.quaternionImsToCam(d2_ref.getQn2b() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr = Imx5.quatToCamAtr(cam_quat_ref);
double [][] ims_ref_xyzatr = {ZERO3, ref_abs_atr};
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (scene == null) {
for (int i = 0; i < 26; i++) {
sb.append("\t---");
}
sb.append("\n");
continue;
}
double timestamp = scene.getTimeStamp();
Did_ins_1 d1 = scene.did_ins_1;
Did_ins_2 d2 = scene.did_ins_2;
Did_pimu d3 = scene.did_pimu;
sb.append(nscene+"\t"+timestamp);
// TODO: try saved, not calculated velocities!
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
// String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
// "\tlong\tlat\talt\tned0\tned1\tned2";
sb.append("\t"+d1.timeOfWeek);
sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[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.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)\tlat\tlong\talt";
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.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.lla[0]+ "\t"+d2.lla[1]+ "\t"+d2.lla[2]);
double [] double_theta = d1.getTheta();
//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
//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);
// 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"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b, new double[3], ims_ortho);
double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
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_xyz_ned = test_xyz_ned(
Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
d2.getQn2b(), // double[] quat_ned,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
double [] cam_xyz_enu = test_xyz_enu(
Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
d2.getQEnu(), // double[] quat_enu,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
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]); //
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
double [][] ims_scene_xyzatr = {ZERO3, scene_abs_atr};
double [] scene_rel_atr=ErsCorrection.combineXYZATR(
ims_scene_xyzatr,
ErsCorrection.invertXYZATR(ims_ref_xyzatr))[1];
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
ErsCorrection.invertXYZATR(ims_ref_xyzatr_enu))[1];
sb.append("\t"+scene_abs_atr[0]+ "\t"+scene_abs_atr[1]+ "\t"+scene_abs_atr[2]); //
sb.append("\t"+scene_rel_atr[0]+ "\t"+scene_rel_atr[1]+ "\t"+scene_rel_atr[2]); //
sb.append("\t"+scene_abs_atr_enu[0]+ "\t"+scene_abs_atr_enu[1]+ "\t"+scene_abs_atr_enu[2]); //
sb.append("\t"+scene_rel_atr_enu[0]+ "\t"+scene_rel_atr_enu[1]+ "\t"+scene_rel_atr_enu[2]); //
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
// 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.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
/*
sb.append("\t"+
cam_theta[0]*180/Math.PI+"\t"+
cam_theta[1]*180/Math.PI+"\t"+
cam_theta[2]*180/Math.PI);
*/
sb.append("\n");
}
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
CalibrationFileManagement.saveStringToFile (
path,
header+"\n"+sb.toString()+"\n"+footer);
}else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
/*
static double [] test_ned(
double [] ned,
double [] enu,
double[] quat_ned,
double[] quat_enu,
double [] ims_mount_atr,
double [] ims_ortho) {
// double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
// double [] enu = new double [] {ned[1], ned[0], -ned[2]};
double [] cam_quat2 =Imx5.quaternionImsToCam(quat,
ims_mount_atr,
ims_ortho);
double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
double [] cam_quat3 =Imx5.quaternionImsToCam(Imx5.quatEnu(quat_ned),
ims_mount_atr,
ims_ortho);
double [] cam_xyz3 = Imx5.applyQuternionTo(cam_quat3, enu, false);
return cam_xyz2;
}
*/
static double [] test_xyz_ned(
double [] ned,
double[] quat_ned,
double [] ims_mount_atr,
double [] ims_ortho) {
double [] cam_quat2 =Imx5.quaternionImsToCam(quat_ned,
ims_mount_atr,
ims_ortho);
return Imx5.applyQuternionTo(cam_quat2, ned, false);
}
static double [] test_xyz_enu(
double [] enu,
double[] quat_enu,
double [] ims_mount_atr,
double [] ims_ortho) {
double [] cam_quat2 =Imx5.quaternionImsToCam(quat_enu,
ims_mount_atr,
ims_ortho);
return Imx5.applyQuternionTo(cam_quat2, enu, false);
}
static double [] test_ned0(
double [] ned,
double[] quat,
double [] ims_mount_atr,
double [] ims_ortho) {
double [][] enu_ned = {{0,1,0},{1, 0, 0},{0, 0, -1}};
Rotation rot_enu_ned = new Rotation(enu_ned, 1E-8);
// double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
Rotation quat_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
double [] enu = new double [] {ned[1], ned[0], -ned[2]};
double [] rslt = new double[3]; // same as ned
rot_enu_ned.applyTo(enu, rslt);
double [] rslt1 = new double[3];
double [] rslt2 = new double[3];
quat_rot.applyTo(ned,rslt1);
// Rotation qr = rot_enu_ned.applyTo(quat_rot);
Rotation qr = quat_rot.applyTo(rot_enu_ned);
qr.applyTo(enu,rslt2);
double [] cam_quat2 =Imx5.quaternionImsToCam(quat,
ims_mount_atr,
ims_ortho);
double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
double [][] reord = new double [16][4];
for (int i = 0; i < reord.length; i++) {
for (int j = 0; j<4; j++) {
boolean neg = ((i >> j) & 1) > 0;
reord[i][j] = 0.5 * (neg? -1 : 1);
}
}
System.out.println ("cam_xyz2: "+
cam_xyz2[0]+", "+ cam_xyz2[1]+", "+ cam_xyz2[2]);
Rotation rot_reord;
Rotation quat_enu_rot;
double [] quat_enu;
double [] cam_quats;
for (int i = 0; i < reord.length; i++) {
rot_reord = new Rotation(
reord[i][0],
reord[i][1],
reord[i][2],
reord[i][3],true);
// quat_enu_rot = rot_reord.applyTo(quat_rot);
quat_enu_rot = quat_rot.applyTo(rot_reord);
quat_enu = new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
quat_enu_rot.getQ2(),
quat_enu_rot.getQ3()};
cam_quats =Imx5.quaternionImsToCam(
quat_enu,
ims_mount_atr,
ims_ortho);
double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
System.out.println (i+": "+
cam_xyzs[0]+", "+ cam_xyzs[1]+", "+ cam_xyzs[2]);
}
return cam_xyz2;
}
}
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ d693f718
......@@ -38,7 +38,16 @@ public class IntersceneMatchParameters {
public boolean ims_rebuild = false; // recalculate INS data (after lag change)
public double ims_offset = 0.0; // IMS data lead relative to images (seconds)
public double gmt_plus = 3.0; // (hrs) filename timestamp is this lower than in IMU log
public double [] ims_ortho = {0.5, -0.5, 0.5, -0.5}; // approximate (90-deg) IMS to camera
public double [] ims_mount_atr = {0, 0, 0}; // IMS mount fine correction (A,T,R around camera axes)
public double [] ims_mount_xyz = {0, 0, 0}; // IMS center in camera coordinates
public double [] getImsMountATR() {
return new double [] {
ims_mount_atr[0] * Math.PI/180,
ims_mount_atr[1] * Math.PI/180,
ims_mount_atr[2] * Math.PI/180};
}
public boolean force_ref_dsi = false; // true;
public boolean force_orientations = false;
public boolean run_ly = false; // will return just after LY adjustments, skipping all output generation
......@@ -217,6 +226,9 @@ public class IntersceneMatchParameters {
public double disp_boost_amount = 2.0;
public double scale_combo_strength = 0.4; // reduce strength when it comes from combo, not DSI-MAIN
public boolean save_debug_images = true; // Save blue sky debug images in model directory
/// IMS intergration
// Some "AGC" to adjust how much to discard
public int margin = 1; // do not use tiles if their centers are closer to the image edge
......@@ -402,17 +414,26 @@ public class IntersceneMatchParameters {
public void dialogQuestions(GenericJTabbedDialog gd) {
// gd.addMessage ("Scene parameters selection");
// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
gd.addTab ("Scene Series", "Processing series of scenes and multi-series sets");
gd.addMessage ("Build series options");
gd.addTab ("IMS", "IMS Integration for interscene matching");
gd.addCheckbox ("Use IMS data", this.ims_use,
"Use INS data for egomotion.");
"Use IMS data for egomotion.");
gd.addCheckbox ("Recalculate IMS", this.ims_rebuild,
"Recalculate scene INS data.");
gd.addNumericField("IMS offset", this.ims_offset, 5,8,"s",
"Positive if the IMS data has lower lag than images.");
gd.addNumericField("GMT plus", this.gmt_plus, 5,8,"hrs",
"Timestamp filename is lower by this hours than in the IMU log.");
gd.addStringField ("IMS ortho mount quaterion", IntersceneMatchParameters.doublesToString(ims_ortho), 80,
"Approximate (90-deg) IMS to camera {0.5, -0.5, 0.5, -0.5}.");
gd.addStringField ("IMS mount ATR correction (deg)", IntersceneMatchParameters.doublesToString(ims_mount_atr), 80,
"IMS mount fine correction (A,T,R around camera axes).");
gd.addStringField ("IMS mount XYZ correction (m)", IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
"MS center (X,Y,Z m) in camera coordinates.");
gd.addTab ("Scene Series", "Processing series of scenes and multi-series sets");
gd.addMessage ("Build series options");
gd.addCheckbox ("Force reference scene DSI calculation", this.force_ref_dsi,
"Calculate reference scene DSI even if the file exists.");
gd.addCheckbox ("Force egomotion calculation", this.force_orientations,
......@@ -1072,6 +1093,9 @@ public class IntersceneMatchParameters {
this.ims_rebuild = gd.getNextBoolean();
this.ims_offset = gd.getNextNumber();
this.gmt_plus = gd.getNextNumber();
this.ims_ortho = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 4);
this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.force_ref_dsi = gd.getNextBoolean();
this.force_orientations = gd.getNextBoolean();
this.run_ly = gd.getNextBoolean();
......@@ -1420,12 +1444,12 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"ims_rebuild", this.ims_rebuild + ""); // boolean
properties.setProperty(prefix+"ims_offset", this.ims_offset+""); // double
properties.setProperty(prefix+"gmt_plus", this.gmt_plus+""); // double
properties.setProperty(prefix+"ims_ortho", IntersceneMatchParameters.doublesToString(this.ims_ortho));
properties.setProperty(prefix+"ims_mount_atr", IntersceneMatchParameters.doublesToString(this.ims_mount_atr));
properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
properties.setProperty(prefix+"force_ref_dsi", this.force_ref_dsi + ""); // boolean
properties.setProperty(prefix+"force_orientations", this.force_orientations + ""); // boolean
properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean
// properties.setProperty(prefix+"readjust_orient", this.readjust_orient + ""); // boolean
// properties.setProperty(prefix+"force_interscene", this.force_interscene + ""); // 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+"generate_egomotion", this.generate_egomotion+""); // boolean
......@@ -1732,6 +1756,12 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"ims_offset")!=null) this.ims_offset=Double.parseDouble(properties.getProperty(prefix+"ims_offset"));
if (properties.getProperty(prefix+"gmt_plus")!=null) this.gmt_plus=Double.parseDouble(properties.getProperty(prefix+"gmt_plus"));
if (properties.getProperty(prefix+"ims_ortho")!=null) this.ims_ortho= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_ortho"),4);
if (properties.getProperty(prefix+"ims_mount_atr")!=null) this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
if (properties.getProperty(prefix+"ims_mount_xyz")!=null) this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);
if (properties.getProperty(prefix+"force_ref_dsi")!=null) this.force_ref_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"force_ref_dsi"));
if (properties.getProperty(prefix+"force_orientations")!=null) this.force_orientations=Boolean.parseBoolean(properties.getProperty(prefix+"force_orientations"));
if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));
......@@ -2068,6 +2098,10 @@ public class IntersceneMatchParameters {
imp.ims_offset = this.ims_offset;
imp.gmt_plus = this.gmt_plus;
imp.ims_ortho = this.ims_ortho.clone();
imp.ims_mount_atr = this.ims_mount_atr.clone();
imp.ims_mount_xyz = this.ims_mount_xyz.clone();
imp.force_ref_dsi = this.force_ref_dsi;
imp.force_orientations = this.force_orientations;
imp.run_ly = this.run_ly;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ d693f718
......@@ -5034,7 +5034,7 @@ public class OpticalFlow {
quadCLTs[ref_index].getImageName()+
quadCLTs[ref_index].correctionsParameters.egomotionSuffix;
String ego_comment = null;
generateEgomotionTable(
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
......@@ -5046,7 +5046,7 @@ public class OpticalFlow {
}
if (ego_show) {
generateEgomotionTable(
Interscene.generateEgomotionTable(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index,// ref_indx,
......@@ -5875,336 +5875,6 @@ public class OpticalFlow {
return quadCLTs[ref_index].getX3dTopDirectory();
}
public static void generateEgomotionTable(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
String path,
String comment) {
boolean use_euler = true;
boolean use_qn2b = true;
String header_img="#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)";
String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
"\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_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";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
String header_cam001= "\ttheta0-001\ttheta1-001\ttheta2-001";
String header_cam010= "\ttheta0-010\ttheta1-010\ttheta2-010";
String header_cam011= "\ttheta0-011\ttheta1-011\ttheta2-011";
String header_cam100= "\ttheta0-100\ttheta1-100\ttheta2-100";
String header_cam101= "\ttheta0-101\ttheta1-101\ttheta2-101";
String header_cam110= "\ttheta0-110\ttheta1-110\ttheta2-110";
String header_cam111= "\ttheta0-111\ttheta1-111\ttheta2-111";
String header_cam200= "\ttheta0-200\ttheta1-200\ttheta2-200";
String header_cam201= "\ttheta0-201\ttheta1-201\ttheta2-201";
String header_cam210= "\ttheta0-210\ttheta1-210\ttheta2-210";
String header_cam211= "\ttheta0-211\ttheta1-211\ttheta2-211";
String header_cam300= "\ttheta0-300\ttheta1-300\ttheta2-300";
String header_cam301= "\ttheta0-301\ttheta1-301\ttheta2-301";
String header_cam310= "\ttheta0-310\ttheta1-310\ttheta2-310";
String header_cam311= "\ttheta0-311\ttheta1-311\ttheta2-311";
String header_cam400= "\ttheta0-400\ttheta1-400\ttheta2-400";
String header_cam401= "\ttheta0-401\ttheta1-401\ttheta2-401";
String header_cam410= "\ttheta0-410\ttheta1-410\ttheta2-410";
String header_cam411= "\ttheta0-411\ttheta1-411\ttheta2-411";
String header_cam500= "\ttheta0-500\ttheta1-500\ttheta2-500";
String header_cam501= "\ttheta0-501\ttheta1-501\ttheta2-501";
String header_cam510= "\ttheta0-510\ttheta1-510\ttheta2-510";
String header_cam511= "\ttheta0-511\ttheta1-511\ttheta2-511";
String header_qn2b000= "\tqn2b0-000\tqn2b1-000\tqn2b2-000";
String header_qn2b001= "\tqn2b0-001\tqn2b1-001\tqn2b2-001";
String header_qn2b010= "\tqn2b0-010\tqn2b1-010\tqn2b2-010";
String header_qn2b011= "\tqn2b0-011\tqn2b1-011\tqn2b2-011";
String header_qn2b100= "\tqn2b0-100\tqn2b1-100\tqn2b2-100";
String header_qn2b101= "\tqn2b0-101\tqn2b1-101\tqn2b2-101";
String header_qn2b110= "\tqn2b0-110\tqn2b1-110\tqn2b2-110";
String header_qn2b111= "\tqn2b0-111\tqn2b1-111\tqn2b2-111";
String header_qn2b200= "\tqn2b0-200\tqn2b1-200\tqn2b2-200";
String header_qn2b201= "\tqn2b0-201\tqn2b1-201\tqn2b2-201";
String header_qn2b210= "\tqn2b0-210\tqn2b1-210\tqn2b2-210";
String header_qn2b211= "\tqn2b0-211\tqn2b1-211\tqn2b2-211";
String header_qn2b300= "\tqn2b0-300\tqn2b1-300\tqn2b2-300";
String header_qn2b301= "\tqn2b0-301\tqn2b1-301\tqn2b2-301";
String header_qn2b310= "\tqn2b0-310\tqn2b1-310\tqn2b2-310";
String header_qn2b311= "\tqn2b0-311\tqn2b1-311\tqn2b2-311";
String header_qn2b400= "\tqn2b0-400\tqn2b1-400\tqn2b2-400";
String header_qn2b401= "\tqn2b0-401\tqn2b1-401\tqn2b2-401";
String header_qn2b410= "\tqn2b0-410\tqn2b1-410\tqn2b2-410";
String header_qn2b411= "\tqn2b0-411\tqn2b1-411\tqn2b2-411";
String header_qn2b500= "\tqn2b0-500\tqn2b1-500\tqn2b2-500";
String header_qn2b501= "\tqn2b0-501\tqn2b1-501\tqn2b2-501";
String header_qn2b510= "\tqn2b0-510\tqn2b1-510\tqn2b2-510";
String header_qn2b511= "\tqn2b0-511\tqn2b1-511\tqn2b2-511";
String header = header_img+header_ins1+header_ins2+header_ins2_extra+header_pimu;
if (use_qn2b) {
header+=
header_qn2b000 + header_qn2b001 + header_qn2b010 + header_qn2b011 +
header_qn2b100 + header_qn2b101 + header_qn2b110 + header_qn2b111 +
header_qn2b200 + header_qn2b201 + header_qn2b210 + header_qn2b211 +
header_qn2b300 + header_qn2b301 + header_qn2b310 + header_qn2b311 +
header_qn2b400 + header_qn2b401 + header_qn2b410 + header_qn2b411 +
header_qn2b500 + header_qn2b501 + header_qn2b510 + header_qn2b511;
}
if (use_euler) {
header+=
header_cam000 + header_cam001 + header_cam010 + header_cam011 +
header_cam100 + header_cam101 + header_cam110 + header_cam111 +
header_cam200 + header_cam201 + header_cam210 + header_cam211 +
header_cam300 + header_cam301 + header_cam310 + header_cam311 +
header_cam400 + header_cam401 + header_cam410 + header_cam411 +
header_cam500 + header_cam501 + header_cam510 + header_cam511;
}
StringBuffer sb = new StringBuffer();
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
scenes_xyzatr[nscene] = new double[2][3];
} else {
String ts = scene.getImageName();
scenes_xyzatr[nscene] = new double [][] {ers_reference.getSceneXYZ(ts),ers_reference.getSceneATR(ts)};
}
}
double [][][] dxyzatr_dt = getVelocitiesFromScenes(
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
ref_index,
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
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 (scene == null) {
for (int i = 0; i < 26; i++) {
sb.append("\t---");
}
sb.append("\n");
continue;
}
double timestamp = scene.getTimeStamp();
Did_ins_1 d1 = scene.did_ins_1;
Did_ins_2 d2 = scene.did_ins_2;
Did_pimu d3 = scene.did_pimu;
sb.append(nscene+"\t"+timestamp);
// TODO: try saved, not calculated velocities!
sb.append("\t"+scenes_xyzatr[nscene][0][0]+"\t"+scenes_xyzatr[nscene][0][1]+"\t"+scenes_xyzatr[nscene][0][2]);
sb.append("\t"+scenes_xyzatr[nscene][1][0]+"\t"+scenes_xyzatr[nscene][1][1]+"\t"+scenes_xyzatr[nscene][1][2]);
sb.append("\t"+dxyzatr_dt[nscene][0][0]+"\t"+dxyzatr_dt[nscene][0][1]+"\t"+dxyzatr_dt[nscene][0][2]);
sb.append("\t"+dxyzatr_dt[nscene][1][0]+"\t"+dxyzatr_dt[nscene][1][1]+"\t"+dxyzatr_dt[nscene][1][2]);
// String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
// "\tlong\tlat\talt\tned0\tned1\tned2";
sb.append("\t"+d1.timeOfWeek);
sb.append("\t"+d1.theta[0]+"\t"+d1.theta[1]+"\t"+d1.theta[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.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)\tlat\tlong\talt";
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.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.lla[0]+ "\t"+d2.lla[1]+ "\t"+d2.lla[2]);
double [] double_theta = new double [] {d1.theta[0], d1.theta[1], d1.theta[2]};
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
double [] double_qn2b = new double [] {d2.qn2b[0],d2.qn2b[1],d2.qn2b[2],d2.qn2b[3]};
double [] double_uvw = new double[] {d2.uvw[0],d2.uvw[1],d2.uvw[2]};
double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false);
double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true);
//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 [] 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"+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_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
// 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.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
// String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
if (use_qn2b) {
double [] cam_qn2b_000 = Imx5.imsQToCamRotations(double_qn2b, 0, false, false);
double [] cam_qn2b_001 = Imx5.imsQToCamRotations(double_qn2b, 0, false, true);
double [] cam_qn2b_010 = Imx5.imsQToCamRotations(double_qn2b, 0, true, false);
double [] cam_qn2b_011 = Imx5.imsQToCamRotations(double_qn2b, 0, true, true);
double [] cam_qn2b_100 = Imx5.imsQToCamRotations(double_qn2b, 1, false, false);
double [] cam_qn2b_101 = Imx5.imsQToCamRotations(double_qn2b, 1, false, true);
double [] cam_qn2b_110 = Imx5.imsQToCamRotations(double_qn2b, 1, true, false);
double [] cam_qn2b_111 = Imx5.imsQToCamRotations(double_qn2b, 1, true, true);
double [] cam_qn2b_200 = Imx5.imsQToCamRotations(double_qn2b, 2, false, false);
double [] cam_qn2b_201 = Imx5.imsQToCamRotations(double_qn2b, 2, false, true);
double [] cam_qn2b_210 = Imx5.imsQToCamRotations(double_qn2b, 2, true, false);
double [] cam_qn2b_211 = Imx5.imsQToCamRotations(double_qn2b, 2, true, true);
double [] cam_qn2b_300 = Imx5.imsQToCamRotations(double_qn2b, 3, false, false);
double [] cam_qn2b_301 = Imx5.imsQToCamRotations(double_qn2b, 3, false, true);
double [] cam_qn2b_310 = Imx5.imsQToCamRotations(double_qn2b, 3, true, false);
double [] cam_qn2b_311 = Imx5.imsQToCamRotations(double_qn2b, 3, true, true);
double [] cam_qn2b_400 = Imx5.imsQToCamRotations(double_qn2b, 4, false, false);
double [] cam_qn2b_401 = Imx5.imsQToCamRotations(double_qn2b, 4, false, true);
double [] cam_qn2b_410 = Imx5.imsQToCamRotations(double_qn2b, 4, true, false);
double [] cam_qn2b_411 = Imx5.imsQToCamRotations(double_qn2b, 4, true, true);
double [] cam_qn2b_500 = Imx5.imsQToCamRotations(double_qn2b, 5, false, false);
double [] cam_qn2b_501 = Imx5.imsQToCamRotations(double_qn2b, 5, false, true);
double [] cam_qn2b_510 = Imx5.imsQToCamRotations(double_qn2b, 5, true, false);
double [] cam_qn2b_511 = Imx5.imsQToCamRotations(double_qn2b, 5, true, true);
sb.append("\t"+cam_qn2b_000[0]+"\t"+cam_qn2b_000[1]+"\t"+cam_qn2b_000[2]);
sb.append("\t"+cam_qn2b_001[0]+"\t"+cam_qn2b_001[1]+"\t"+cam_qn2b_001[2]);
sb.append("\t"+cam_qn2b_010[0]+"\t"+cam_qn2b_010[1]+"\t"+cam_qn2b_010[2]);
sb.append("\t"+cam_qn2b_011[0]+"\t"+cam_qn2b_011[1]+"\t"+cam_qn2b_011[2]);
sb.append("\t"+cam_qn2b_100[0]+"\t"+cam_qn2b_100[1]+"\t"+cam_qn2b_100[2]);
sb.append("\t"+cam_qn2b_101[0]+"\t"+cam_qn2b_101[1]+"\t"+cam_qn2b_101[2]);
sb.append("\t"+cam_qn2b_110[0]+"\t"+cam_qn2b_110[1]+"\t"+cam_qn2b_110[2]);
sb.append("\t"+cam_qn2b_111[0]+"\t"+cam_qn2b_111[1]+"\t"+cam_qn2b_111[2]);
sb.append("\t"+cam_qn2b_200[0]+"\t"+cam_qn2b_200[1]+"\t"+cam_qn2b_200[2]);
sb.append("\t"+cam_qn2b_201[0]+"\t"+cam_qn2b_201[1]+"\t"+cam_qn2b_201[2]);
sb.append("\t"+cam_qn2b_210[0]+"\t"+cam_qn2b_210[1]+"\t"+cam_qn2b_210[2]);
sb.append("\t"+cam_qn2b_211[0]+"\t"+cam_qn2b_211[1]+"\t"+cam_qn2b_211[2]);
sb.append("\t"+cam_qn2b_300[0]+"\t"+cam_qn2b_300[1]+"\t"+cam_qn2b_300[2]);
sb.append("\t"+cam_qn2b_301[0]+"\t"+cam_qn2b_301[1]+"\t"+cam_qn2b_301[2]);
sb.append("\t"+cam_qn2b_310[0]+"\t"+cam_qn2b_310[1]+"\t"+cam_qn2b_310[2]);
sb.append("\t"+cam_qn2b_311[0]+"\t"+cam_qn2b_311[1]+"\t"+cam_qn2b_311[2]);
sb.append("\t"+cam_qn2b_400[0]+"\t"+cam_qn2b_400[1]+"\t"+cam_qn2b_400[2]);
sb.append("\t"+cam_qn2b_401[0]+"\t"+cam_qn2b_401[1]+"\t"+cam_qn2b_401[2]);
sb.append("\t"+cam_qn2b_410[0]+"\t"+cam_qn2b_410[1]+"\t"+cam_qn2b_410[2]);
sb.append("\t"+cam_qn2b_411[0]+"\t"+cam_qn2b_411[1]+"\t"+cam_qn2b_411[2]);
sb.append("\t"+cam_qn2b_500[0]+"\t"+cam_qn2b_500[1]+"\t"+cam_qn2b_500[2]);
sb.append("\t"+cam_qn2b_501[0]+"\t"+cam_qn2b_501[1]+"\t"+cam_qn2b_501[2]);
sb.append("\t"+cam_qn2b_510[0]+"\t"+cam_qn2b_510[1]+"\t"+cam_qn2b_510[2]);
sb.append("\t"+cam_qn2b_511[0]+"\t"+cam_qn2b_511[1]+"\t"+cam_qn2b_511[2]);
}
if (use_euler) {
double [] cam_theta_000 = Imx5.imsToCamRotations(double_theta, 0, false, false);
double [] cam_theta_001 = Imx5.imsToCamRotations(double_theta, 0, false, true);
double [] cam_theta_010 = Imx5.imsToCamRotations(double_theta, 0, true, false);
double [] cam_theta_011 = Imx5.imsToCamRotations(double_theta, 0, true, true);
double [] cam_theta_100 = Imx5.imsToCamRotations(double_theta, 1, false, false);
double [] cam_theta_101 = Imx5.imsToCamRotations(double_theta, 1, false, true);
double [] cam_theta_110 = Imx5.imsToCamRotations(double_theta, 1, true, false);
double [] cam_theta_111 = Imx5.imsToCamRotations(double_theta, 1, true, true);
double [] cam_theta_200 = Imx5.imsToCamRotations(double_theta, 2, false, false);
double [] cam_theta_201 = Imx5.imsToCamRotations(double_theta, 2, false, true);
double [] cam_theta_210 = Imx5.imsToCamRotations(double_theta, 2, true, false);
double [] cam_theta_211 = Imx5.imsToCamRotations(double_theta, 2, true, true);
double [] cam_theta_300 = Imx5.imsToCamRotations(double_theta, 3, false, false);
double [] cam_theta_301 = Imx5.imsToCamRotations(double_theta, 3, false, true);
double [] cam_theta_310 = Imx5.imsToCamRotations(double_theta, 3, true, false);
double [] cam_theta_311 = Imx5.imsToCamRotations(double_theta, 3, true, true);
double [] cam_theta_400 = Imx5.imsToCamRotations(double_theta, 4, false, false);
double [] cam_theta_401 = Imx5.imsToCamRotations(double_theta, 4, false, true);
double [] cam_theta_410 = Imx5.imsToCamRotations(double_theta, 4, true, false);
double [] cam_theta_411 = Imx5.imsToCamRotations(double_theta, 4, true, true);
double [] cam_theta_500 = Imx5.imsToCamRotations(double_theta, 5, false, false);
double [] cam_theta_501 = Imx5.imsToCamRotations(double_theta, 5, false, true);
double [] cam_theta_510 = Imx5.imsToCamRotations(double_theta, 5, true, false);
double [] cam_theta_511 = Imx5.imsToCamRotations(double_theta, 5, true, true);
sb.append("\t"+cam_theta_000[0]+"\t"+cam_theta_000[1]+"\t"+cam_theta_000[2]);
sb.append("\t"+cam_theta_001[0]+"\t"+cam_theta_001[1]+"\t"+cam_theta_001[2]);
sb.append("\t"+cam_theta_010[0]+"\t"+cam_theta_010[1]+"\t"+cam_theta_010[2]);
sb.append("\t"+cam_theta_011[0]+"\t"+cam_theta_011[1]+"\t"+cam_theta_011[2]);
sb.append("\t"+cam_theta_100[0]+"\t"+cam_theta_100[1]+"\t"+cam_theta_100[2]);
sb.append("\t"+cam_theta_101[0]+"\t"+cam_theta_101[1]+"\t"+cam_theta_101[2]);
sb.append("\t"+cam_theta_110[0]+"\t"+cam_theta_110[1]+"\t"+cam_theta_110[2]);
sb.append("\t"+cam_theta_111[0]+"\t"+cam_theta_111[1]+"\t"+cam_theta_111[2]);
sb.append("\t"+cam_theta_200[0]+"\t"+cam_theta_200[1]+"\t"+cam_theta_200[2]);
sb.append("\t"+cam_theta_201[0]+"\t"+cam_theta_201[1]+"\t"+cam_theta_201[2]);
sb.append("\t"+cam_theta_210[0]+"\t"+cam_theta_210[1]+"\t"+cam_theta_210[2]);
sb.append("\t"+cam_theta_211[0]+"\t"+cam_theta_211[1]+"\t"+cam_theta_211[2]);
sb.append("\t"+cam_theta_300[0]+"\t"+cam_theta_300[1]+"\t"+cam_theta_300[2]);
sb.append("\t"+cam_theta_301[0]+"\t"+cam_theta_301[1]+"\t"+cam_theta_301[2]);
sb.append("\t"+cam_theta_310[0]+"\t"+cam_theta_310[1]+"\t"+cam_theta_310[2]);
sb.append("\t"+cam_theta_311[0]+"\t"+cam_theta_311[1]+"\t"+cam_theta_311[2]);
sb.append("\t"+cam_theta_400[0]+"\t"+cam_theta_400[1]+"\t"+cam_theta_400[2]);
sb.append("\t"+cam_theta_401[0]+"\t"+cam_theta_401[1]+"\t"+cam_theta_401[2]);
sb.append("\t"+cam_theta_410[0]+"\t"+cam_theta_410[1]+"\t"+cam_theta_410[2]);
sb.append("\t"+cam_theta_411[0]+"\t"+cam_theta_411[1]+"\t"+cam_theta_411[2]);
sb.append("\t"+cam_theta_500[0]+"\t"+cam_theta_500[1]+"\t"+cam_theta_500[2]);
sb.append("\t"+cam_theta_501[0]+"\t"+cam_theta_501[1]+"\t"+cam_theta_501[2]);
sb.append("\t"+cam_theta_510[0]+"\t"+cam_theta_510[1]+"\t"+cam_theta_510[2]);
sb.append("\t"+cam_theta_511[0]+"\t"+cam_theta_511[1]+"\t"+cam_theta_511[2]);
}
/*
sb.append("\t"+
cam_theta[0]*180/Math.PI+"\t"+
cam_theta[1]*180/Math.PI+"\t"+
cam_theta[2]*180/Math.PI);
*/
sb.append("\n");
}
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
CalibrationFileManagement.saveStringToFile (
path,
header+"\n"+sb.toString()+"\n"+footer);
}else{
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
public void adjustLYSeries(
QuadCLT quadCLT_main, // update extrinsics here too
QuadCLT [] quadCLTs,
......
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