Skip to content

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

Initial ERS from IMS

parent 99fcbcc4
Show whitespace changes
Inline Side-by-side
Showing with 209 additions and 507 deletions
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ e9aa9780
......@@ -187,25 +187,11 @@ public class Imx5 {
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);
public static double [] quaternionImsToCam(
double [] quat, // ims_to_ned
double [] ims_atr, // -> mount_to_cam
double [] quat_ort) { // -> ims_to_mount_ortho
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]);
......@@ -214,75 +200,24 @@ public class Imx5 {
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
}
// from ims frame to camera frame
public static double [] quaternionImsToCam(
double[]quat,
double [] ims_atr,
double [] quat_ort) {
double [] ims_atr, // -> mount_to_cam
double [] quat_ort) { // -> ims_to_mount_ortho
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);
Rotation cam_quat = mount_to_cam.applyTo(ims_to_mount_ortho);
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;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder ro = RotationOrder.XYZ;
switch (ord) {
case 0: ro = RotationOrder.XYZ; break;
case 1: ro = RotationOrder.XZY; break;
case 2: ro = RotationOrder.YXZ; break;
case 3: ro = RotationOrder.YZX; break;
case 4: ro = RotationOrder.ZXY; break;
case 5: ro = RotationOrder.ZYX; break;
}
RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
double [] angles1 = cam_to_ims.getAngles(ro1, rc);
Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
double [] angles = comb_rot.getAngles(ro1, rc);
return angles;
}
public static double [] imsQToCamRotations(double [] ims_qn2b, int ord, boolean rev_order, boolean rev_matrix ) {
RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder ro = RotationOrder.XYZ;
switch (ord) {
case 0: ro = RotationOrder.XYZ; break;
case 1: ro = RotationOrder.XZY; break;
case 2: ro = RotationOrder.YXZ; break;
case 3: ro = RotationOrder.YZX; break;
case 4: ro = RotationOrder.ZXY; break;
case 5: ro = RotationOrder.ZYX; break;
}
RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
double [] angles1 = cam_to_ims.getAngles(ro1, rc);
// Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
// public Rotation(double q0, double q1, double q2, double q3, boolean needsNormalization) {
Rotation ims_rot = new Rotation(ims_qn2b[0],ims_qn2b[1],ims_qn2b[2],ims_qn2b[3],true);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
double [] angles = comb_rot.getAngles(ro1, rc);
return angles;
}
public static double [] nedFromLla (double [] lla, double [] lla_ref) {
double [] ned = new double[] {
EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
......
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ e9aa9780
......@@ -260,6 +260,13 @@ public class ErsCorrection extends GeometryCorrection {
this.ers_wxyz_center_dt = ers_xyz_dt;
this.ers_watr_center_dt = ers_atr_dt;
}
public void setErsDt(
double [][] ers_xyzatr_dt) {
this.ers_wxyz_center_dt = ers_xyzatr_dt[0];
this.ers_watr_center_dt = ers_xyzatr_dt[1];
}
public void setErsDt_test(
double [] ers_xyz_dt,
double [] ers_atr_dt) {
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ e9aa9780
......@@ -277,6 +277,7 @@ public class Interscene {
return earliest_scene2;
}
public static int setInitialOrientationsIms(
final CLTParameters clt_parameters,
int min_num_scenes,
......@@ -290,6 +291,7 @@ public class Interscene {
final int threadsMax, // int threadsMax,
final boolean updateStatus,
final int debugLevel) {
// double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
double maximal_series_rms = 0.0;
double min_ref_str = clt_parameters.imp.min_ref_str;
boolean ref_need_lma = clt_parameters.imp.ref_need_lma;
......@@ -300,7 +302,10 @@ public class Interscene {
boolean ims_use = clt_parameters.imp.ims_use;
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] quat_ims_cam = Imx5.quaternionImsToCam(
// ims_mount_atr, // new double[] {0, 0.13, 0},
// ims_ortho);
double boost_max_short = 2.0; //
double boost_zoom_short = 1.5; //
......@@ -380,7 +385,7 @@ public class Interscene {
boolean after_spiral = false;
boolean got_spiral = false;
// int search_rad = clt_parameters.imp.search_rad; // 10;
boolean scale_ims_velocities = true;
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
// apply correction to orientation
double [] cam_quat_ref_enu =Imx5.quaternionImsToCam(d2_ref.getQEnu() ,
......@@ -389,11 +394,19 @@ public class Interscene {
double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
double [][] last_corr_xyzatr = {ZERO3,ZERO3};
double [][] cam_dxyzatr_ref = quadCLTs[ref_index].getDxyzatrIms(
clt_parameters,
scale_ims_velocities); // scale IMS velocities
ers_reference.addScene(quadCLTs[ref_index].getImageName(), // add reference scene (itself) too
scenes_xyzatr[ref_index][0],
scenes_xyzatr[ref_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3); // ers_scene.getErsATR_dt()
cam_dxyzatr_ref[0], // ZERO3, // ers_scene.getErsXYZ_dt(),
cam_dxyzatr_ref[1]); // ZERO3);// ers_scene.getErsATR_dt()
// Will be used in prepareLMA()
quadCLTs[ref_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
cam_dxyzatr_ref));
for (int scene_index = ref_index - 1; scene_index >= earliest_scene ; scene_index--) {
if ((ref_index - scene_index) >= max_num_scenes){
earliest_scene = scene_index + 1;
......@@ -462,6 +475,15 @@ public class Interscene {
" pixels");
}
}
double [][] cam_dxyzatr = quadCLTs[scene_index].getDxyzatrIms(
clt_parameters,
scale_ims_velocities); // scale IMS velocities
// Will be used in prepareLMA()
quadCLTs[scene_index].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
cam_dxyzatr));
// Does not use motion blur for reference scene here!
scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters,
......@@ -562,12 +584,15 @@ public class Interscene {
}
}
// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[scene_index].getErsCorrection().getErsXYZATR_dt(
));
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr[scene_index][0],
scenes_xyzatr[scene_index][1],
ZERO3, // ers_scene.getErsXYZ_dt(),
ZERO3 // ers_scene.getErsATR_dt()
adjusted_xyzatr_dt[0], // ZERO3, // ers_scene.getErsXYZ_dt(),
adjusted_xyzatr_dt[1] // ZERO3 // ers_scene.getErsATR_dt()
);
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
......@@ -1103,6 +1128,7 @@ public class Interscene {
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
double [][][] scenes_xyzatr = new double [quadCLTs.length][][]; // previous scene relative to the next one
double [][][] dxyzatr_dt = new double [quadCLTs.length][][];
scenes_xyzatr[ref_index] = new double[2][3]; // all zeros
// should have at least next or previous non-null
......@@ -1132,6 +1158,7 @@ public class Interscene {
}
scenes_xyzatr[nscene] = new double[][] {ers_reference.getSceneXYZ(ts), ers_reference.getSceneATR(ts)};
}
dxyzatr_dt[nscene] = ers_reference.getSceneErsXYZATR_dt(quadCLTs[nscene].getImageName());
if (debug_ers) {
System.out.println(String.format("%3d xyz: %9.5f %9.5f %9.5f atr: %9.5f %9.5f %9.5f",
nscene,
......@@ -1139,7 +1166,6 @@ public class Interscene {
scenes_xyzatr[nscene][1][0], scenes_xyzatr[nscene][1][1], scenes_xyzatr[nscene][1][2]));
}
}
double [][][] dxyzatr_dt;
switch (ers_mode) {
case 1 :
dxyzatr_dt = OpticalFlow.getVelocitiesFromScenes(
......@@ -1150,13 +1176,13 @@ public class Interscene {
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
break;
case 2: // read from ims
default:
dxyzatr_dt = new double [quadCLTs.length][][];
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// read from map (ts)
// divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
}
// case 2: // read from ims
default: // do nothing - already read
// dxyzatr_dt = new double [quadCLTs.length][][];
// for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// // read from map (ts)
// // divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
// }
}
if (debug_ers) {
......@@ -1214,7 +1240,7 @@ public class Interscene {
double [] scene_xyz_pre = ZERO3;
double [] scene_atr_pre = ZERO3;
// find correct signs when setting. dxyzatr_dt[][] is also used for motion blur (correctly)
/*
double [][] scaled_dxyzatr_dt = new double[2][3];
for (int i = 0; i < scaled_dxyzatr_dt.length; i++) {
for (int j = 0; j < scaled_dxyzatr_dt[i].length; j++) {
......@@ -1226,6 +1252,12 @@ public class Interscene {
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
scaled_dxyzatr_dt[0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt,
scaled_dxyzatr_dt[1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[nscene]));
*/
if (dbg_mb_img != null) {
boolean show_corrected = false;
if (nscene == debug_scene) {
......@@ -1304,15 +1336,14 @@ public class Interscene {
// double [][] dxyzatr_dt_ref = getVelocities( // was already set
// quadCLTs, // QuadCLT [] quadCLTs,
// ref_index); // int nscene)
double [][] dxyzatr_dt_ref = dxyzatr_dt[nscene];
mb_vectors_ref = OpticalFlow.getMotionBlur(
quadCLTs[ref_index], // QuadCLT ref_scene,
quadCLTs[ref_index], // QuadCLT scene, // can be the same as ref_scene
ref_pXpYD, // double [][] ref_pXpYD, // here it is scene, not reference!
ZERO3, // double [] camera_xyz,
ZERO3, // double [] camera_atr,
dxyzatr_dt_ref[0], // double [] camera_xyz_dt,
dxyzatr_dt_ref[1], // double [] camera_atr_dt,
dxyzatr_dt[nscene][0], // double [] camera_xyz_dt,
dxyzatr_dt[nscene][1], // double [] camera_atr_dt,
0, // int shrink_gaps, // will gaps, but not more that grow by this
debugLevel); // int debug_level)
}
......@@ -1477,12 +1508,24 @@ public class Interscene {
}
}
// overwrite old data
// undo velocities->ers scaling
double [][] adjusted_xyzatr_dt = QuadCLTCPU.scaleDtFromErs(
clt_parameters,
quadCLTs[nscene].getErsCorrection().getErsXYZATR_dt(
));
// ers_reference.addScene(ts,
// scenes_xyzatr[nscene][0],
// scenes_xyzatr[nscene][1],
// quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(), // same as dxyzatr_dt[nscene][0], just keep for future adjustments?
// quadCLTs[nscene].getErsCorrection().getErsATR_dt() // same as dxyzatr_dt[nscene][1], just keep for future adjustments?
// );
ers_reference.addScene(ts,
scenes_xyzatr[nscene][0],
scenes_xyzatr[nscene][1],
quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(), // same as dxyzatr_dt[nscene][0], just keep for future adjustments?
quadCLTs[nscene].getErsCorrection().getErsATR_dt() // same as dxyzatr_dt[nscene][1], just keep for future adjustments?
adjusted_xyzatr_dt[0],
adjusted_xyzatr_dt[1]
);
if (lma_rms[0] > maximal_series_rms) {
maximal_series_rms = lma_rms[0];
}
......@@ -1717,12 +1760,15 @@ public class Interscene {
for (int i:ErsCorrection.DP_ERS_INDICES) {
param_select_mod[i] = false;
}
// TODO Removed on 11/23/2023 for use with IMS. Set reasonable data if IMS is not used
/*
// now just copy ers from the reference
ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
// when disabled - scene ers from the same as reference one
ers_scene.ers_watr_center_dt = ers_ref.ers_watr_center_dt.clone();
ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
*/
}
// TODO: save ers_scene.ers_watr_center_dt and ers_scene.ers_wxyz_center_dt
intersceneLma.prepareLMA(
......@@ -3727,339 +3773,6 @@ public class Interscene {
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
return tp_tasks_ref;
}
@Deprecated
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(
......@@ -4110,8 +3823,10 @@ public class Interscene {
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_camv="\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr";
String header = header_ts+(use_processed?header_img:"")+header_ins1+header_ins2+header_ins2_extra+header_pimu;
String header = header_ts+(use_processed?header_img:"")+header_ins1+
header_ins2+header_ins2_extra+header_pimu+header_camv;
StringBuffer sb = new StringBuffer();
double [][][] dxyzatr_dt = null;
......@@ -4124,6 +3839,11 @@ public class Interscene {
scenes_xyzatr, // double [][][] scenes_xyzatr,
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
}
double [] quat_ims_cam = Imx5.quaternionImsToCam(
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
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},
......@@ -4212,11 +3932,20 @@ public class Interscene {
d2_ref.getQn2b(), // double[] quat_ned,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
double [] cam_xyz_enu = test_xyz_enu(
/*
double [] cam_xyz_enu0 = test_xyz_enu(
Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
*/
double [] cam_xyz_enu = Imx5.applyQuternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
......@@ -4246,9 +3975,22 @@ public class Interscene {
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));
/*
double [] omegas=new double[] {d3.theta[0]/d3.dt, d3.theta[1]/d3.dt, d3.theta[2]/d3.dt};
double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
sb.append("\t"+cam_dxyz[0]+ "\t"+cam_dxyz[1]+ "\t"+cam_dxyz[2]);
// a (around Y),t (around X), r (around Z)
sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]);
*/
double [][] cam_dxyzatr = scene.getDxyzatrIms(clt_parameters, false); // raw, do not scale
sb.append("\t"+cam_dxyzatr[0][0]+ "\t"+cam_dxyzatr[0][1]+ "\t"+cam_dxyzatr[0][2]);
sb.append("\t"+cam_dxyzatr[1][0]+ "\t"+cam_dxyzatr[1][1]+ "\t"+cam_dxyzatr[1][2]);
// add lpf
sb.append("\n");
}
......@@ -4287,71 +4029,4 @@ public class Interscene {
}
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 @ e9aa9780
......@@ -41,6 +41,9 @@ public class IntersceneMatchParameters {
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 [] ims_scale_xyz = {1.1, 1.1, 1.1};
public double [] ims_scale_atr = {1.1, 1.1, 1.1};
public boolean sfm_use = true; // use SfM to improve depth map
public double sfm_min_base = 2.0; // use SfM if baseline exceeds this
......@@ -462,6 +465,11 @@ public class IntersceneMatchParameters {
gd.addStringField ("IMS mount XYZ correction (m)", IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
"MS center (X,Y,Z m) in camera coordinates.");
gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,
"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,
"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1'.");
gd.addTab ("SfM", "Structure from Motion to improve depth map for the lateral views");
gd.addCheckbox ("Use SfM", this.sfm_use,
"Use SfM for the depth map enhancement for laterally moving camera.");
......@@ -1186,6 +1194,8 @@ public class IntersceneMatchParameters {
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.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.sfm_use = gd.getNextBoolean();
this.sfm_min_base = gd.getNextNumber();
this.sfm_min_gain = gd.getNextNumber();
......@@ -1566,6 +1576,8 @@ public class IntersceneMatchParameters {
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+"ims_scale_xyz", IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));
properties.setProperty(prefix+"ims_scale_atr", IntersceneMatchParameters.doublesToString(this.ims_scale_atr));
properties.setProperty(prefix+"sfm_use", this.sfm_use + ""); // boolean
properties.setProperty(prefix+"sfm_min_base", this.sfm_min_base+""); // double
properties.setProperty(prefix+"sfm_min_gain", this.sfm_min_gain+""); // double
......@@ -1906,6 +1918,8 @@ public class IntersceneMatchParameters {
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+"ims_scale_xyz")!=null) this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);
if (properties.getProperty(prefix+"ims_scale_atr")!=null) this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);
if (properties.getProperty(prefix+"sfm_use")!=null) this.sfm_use=Boolean.parseBoolean(properties.getProperty(prefix+"sfm_use"));
if (properties.getProperty(prefix+"sfm_min_base")!=null) this.sfm_min_base=Double.parseDouble(properties.getProperty(prefix+"sfm_min_base"));
if (properties.getProperty(prefix+"sfm_min_gain")!=null) this.sfm_min_gain=Double.parseDouble(properties.getProperty(prefix+"sfm_min_gain"));
......@@ -2273,6 +2287,8 @@ public class IntersceneMatchParameters {
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.ims_scale_xyz = this.ims_scale_xyz.clone();
imp.ims_scale_atr = this.ims_scale_atr.clone();
imp.sfm_use = this.sfm_use;
imp.sfm_min_base = this.sfm_min_base;
imp.sfm_min_gain = this.sfm_min_gain;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ e9aa9780
......@@ -4773,6 +4773,20 @@ public class OpticalFlow {
int center_index =quadCLTs[last_index].getReferenceIndex(null);
if (center_index == -1) {
build_ref_dsi = true;
} else {
QuadCLT try_ref_scene = (QuadCLT) quadCLT_main.spawnQuadCLT( // will conditionImageSet
quadCLTs[last_index].getReferenceTimestamp(), // set_channels[last_index].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel-2);
if ((try_ref_scene == null) || (try_ref_scene.getDLS() == null)) {
if (debugLevel >-2) {
System.out.println("DSI data for scene "+quadCLTs[last_index].getReferenceTimestamp()+
" does not exist, forcing initial orientation.");
}
force_initial_orientations = true;
}
}
} else {
if ((quadCLTs[last_index] == null) || !quadCLTs[last_index].dsiExists()) {
......@@ -4995,7 +5009,7 @@ public class OpticalFlow {
ref_index =quadCLTs[last_index].getReferenceIndex(quadCLTs);
quadCLTs[ref_index].restoreInterProperties(null, false, debugLevel); //null
int [] first_last_index = quadCLTs[ref_index].getFirstLastIndex(quadCLTs);
earliest_scene = first_last_index[0];
earliest_scene = first_last_index[0]; // null pointer if does not exist
last_index = first_last_index[1];
}
// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
......@@ -5188,8 +5202,12 @@ public class OpticalFlow {
}
boolean disable_ers = (quadCLTs[ref_index].getNumOrient() < 2); // first orient - no ERS!
boolean ers_from_ims = false; // change later
int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
ers_mode = 1; // TODO: remove later!
// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
int ers_mode = 0; // keep
// with IMS it is already set during initial orientation. In non-IMS mode
if (!ers_from_ims && (quadCLTs[ref_index].getNumOrient() < 2)) {
ers_mode = 1; // calculate velocity
}
earliest_scene = Interscene.reAdjustPairsLMAInterscene( // after combo dsi is available and preliminary poses are known
clt_parameters, // CLTParameters clt_parameters,
mb_max_gain, // double mb_max_gain,
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ e9aa9780
......@@ -196,6 +196,57 @@ public class QuadCLTCPU {
return timestamp_reference;
}
public double [][] getDxyzatrIms(
CLTParameters clt_parameters,
boolean scale) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] quat_ims_cam = Imx5.quaternionImsToCam(
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] double_uvw = did_ins_2.getUvw();
double [] omegas=new double[] {did_pimu.theta[0]/did_pimu.dt,
did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt};
double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
// a (around Y),t (around X), r (around Z)
double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
if (scale) {
for (int i = 0; i < 3; i++) {
dxyzatyr[0][i]*=clt_parameters.imp.ims_scale_xyz[i];
dxyzatyr[1][i]*=clt_parameters.imp.ims_scale_atr[i];
}
}
return dxyzatyr;
}
public static double [][] scaleDtToErs(
CLTParameters clt_parameters,
double [][] cam_dxyzatr){
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
double [][] scaled_dxyzatr = new double[2][3];
for (int i = 0; i < scaled_dxyzatr.length; i++) {
for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
scaled_dxyzatr[i][j] = cam_dxyzatr[i][j]* dbg_scale_dt[i][j];
}
}
return scaled_dxyzatr;
}
public static double [][] scaleDtFromErs(
CLTParameters clt_parameters,
double [][] scaled_dxyzatr){
double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
double [][] cam_dxyzatr = new double[2][3];
for (int i = 0; i < scaled_dxyzatr.length; i++) {
for (int j = 0; j < scaled_dxyzatr[i].length; j++) {
cam_dxyzatr[i][j] = scaled_dxyzatr[i][j] / dbg_scale_dt[i][j];
}
}
return cam_dxyzatr;
}
/**
* If this scene has a reference to one of the scenes array, return its index in the array
* otherwise return -1 if there is no pointer to reference scene and -2 if pointer is not
......
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