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
Expand all Hide 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
......@@ -186,26 +186,12 @@ 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]);
......@@ -213,76 +199,25 @@ public class Imx5 {
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()};
}
// 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/IntersceneMatchParameters.java
View file @ e9aa9780
......@@ -41,7 +41,10 @@ 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
public double sfm_min_gain = 5.0; // Minimal SfM gain to apply SfM to the depth map
......@@ -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