Skip to content

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

unfinished mod

parent 2cb7df5a
Show whitespace changes
Inline Side-by-side
Showing with 248 additions and 9 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 93c00b54
......@@ -27,8 +27,6 @@ import java.util.ArrayList;
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;
......@@ -5365,14 +5363,14 @@ public class Interscene {
enu,
false);
}
double lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
double lambda_max = 100;
double rms_diff = 0.001;
int num_iter = 20;
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = 0.25;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
int debug_level = 1;
QuaternionLma quaternionLma = new QuaternionLma();
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 93c00b54
......@@ -435,6 +435,15 @@ public class IntersceneMatchParameters {
public boolean readjust_xy_ims = true; // false;
public double reg_weight_xy = 0; // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values
// Quaternion LMA parameters
public double quat_lambda = 0.1;
public double quat_lambda_scale_good = 0.5;
public double quat_lambda_scale_bad = 8.0;
public double quat_lambda_max = 100;
public double quat_rms_diff = 0.001;
public int quat_num_iter = 20;
public double quat_reg_w = 0.25;
public boolean stereo_merge = true;
public int stereo_gap = 32; // pixels between right and left frames
public double stereo_intereye = 63.5; // mm
......@@ -1241,6 +1250,22 @@ public class IntersceneMatchParameters {
gd.addNumericField("LMA regularization weight", this.reg_weight_xy, 5,7,"",
"Regularization weight for LMA");
gd.addTab("Quaternion LMA","Parameters fitting rotation to two sets of vectors");
gd.addNumericField("LMA lambda", this.quat_lambda, 6,8,"",
"Initial value of the LMA lambda");
gd.addNumericField("Scale lambda after successful LMA iteration", this.quat_lambda_scale_good, 3,5,"",
"Scale lambda (reduce) if the new RMSE is lower than the previous one.");
gd.addNumericField("Scale lambda after failed LMA iteration", this.quat_lambda_scale_bad, 3,5,"",
"Scale lambda (increase) if the new RMSE is higher than the previous one.");
gd.addNumericField("Maximal value of lambda to try", this.quat_lambda_max, 2,7,"",
"Fail LMA if the result is still worse than before parameters were updates.");
gd.addNumericField("Minimal relative RMSE improvement", this.quat_rms_diff, 5,7,"",
"Exit LMA iterations if relative RMSE improvement drops below this value.");
gd.addNumericField("Maximal number of LMA iterations", this.quat_num_iter, 0,3,"",
"A hard limit on LMA iterations.");
gd.addNumericField("Regularization weight", this.quat_reg_w, 3,5,"",
"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");
gd.addTab("Stereo/Video","Parameters for stereo video generation");
gd.addMessage ("Stereo");
......@@ -1673,6 +1698,14 @@ public class IntersceneMatchParameters {
this.readjust_xy_ims = gd.getNextBoolean();
this.reg_weight_xy = gd.getNextNumber();
this.quat_lambda = gd.getNextNumber();
this.quat_lambda_scale_good = gd.getNextNumber();
this.quat_lambda_scale_bad = gd.getNextNumber();
this.quat_lambda_max = gd.getNextNumber();
this.quat_rms_diff = gd.getNextNumber();
this.quat_num_iter = (int) gd.getNextNumber();
this.quat_reg_w = gd.getNextNumber();
if (stereo_views.length > 0) {
int i = gd.getNextChoiceIndex();
if (i > 0) {
......@@ -2105,6 +2138,14 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"readjust_xy_ims", this.readjust_xy_ims+""); // boolean
properties.setProperty(prefix+"reg_weight_xy", this.reg_weight_xy+""); // double
properties.setProperty(prefix+"quat_lambda", this.quat_lambda+""); // double
properties.setProperty(prefix+"quat_lambda_scale_good",this.quat_lambda_scale_good+"");// double
properties.setProperty(prefix+"quat_lambda_scale_bad",this.quat_lambda_scale_bad+"");// double
properties.setProperty(prefix+"quat_lambda_max", this.quat_lambda_max+""); // double
properties.setProperty(prefix+"quat_rms_diff", this.quat_rms_diff+""); // double
properties.setProperty(prefix+"quat_num_iter", this.quat_num_iter+""); // int
properties.setProperty(prefix+"quat_reg_w", this.quat_reg_w+""); // double
properties.setProperty(prefix+"stereo_merge", this.stereo_merge+""); // boolean
properties.setProperty(prefix+"stereo_gap", this.stereo_gap+""); // int
properties.setProperty(prefix+"stereo_intereye", this.stereo_intereye+""); // double
......@@ -2498,6 +2539,15 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"readjust_xy_ims")!=null) this.readjust_xy_ims=Boolean.parseBoolean(properties.getProperty(prefix+"readjust_xy_ims"));
if (properties.getProperty(prefix+"reg_weight_xy")!=null) this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
if (properties.getProperty(prefix+"quat_lambda")!=null) this.quat_lambda=Double.parseDouble(properties.getProperty(prefix+"quat_lambda"));
if (properties.getProperty(prefix+"quat_lambda_scale_good")!=null)this.quat_lambda_scale_good=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_good"));
if (properties.getProperty(prefix+"quat_lambda_scale_bad")!=null)this.quat_lambda_scale_bad=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_bad"));
if (properties.getProperty(prefix+"quat_lambda_max")!=null) this.quat_lambda_max=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_max"));
if (properties.getProperty(prefix+"quat_rms_diff")!=null) this.quat_rms_diff=Double.parseDouble(properties.getProperty(prefix+"quat_rms_diff"));
if (properties.getProperty(prefix+"reg_weight_xy")!=null) this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));
if (properties.getProperty(prefix+"quat_num_iter")!=null) this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
if (properties.getProperty(prefix+"quat_reg_w")!=null) this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));
if (properties.getProperty(prefix+"stereo_merge")!=null) this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
if (properties.getProperty(prefix+"stereo_gap")!=null) this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
if (properties.getProperty(prefix+"stereo_intereye")!=null) this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye"));
......@@ -2901,6 +2951,14 @@ public class IntersceneMatchParameters {
imp.readjust_xy_ims = this.readjust_xy_ims;
imp.reg_weight_xy = this.reg_weight_xy;
imp.quat_lambda = this.quat_lambda;
imp.quat_lambda_scale_good= this.quat_lambda_scale_good;
imp.quat_lambda_scale_bad = this.quat_lambda_scale_bad;
imp.quat_lambda_max = this.quat_lambda_max;
imp.quat_rms_diff = this.quat_rms_diff;
imp.quat_num_iter = this.quat_num_iter;
imp.quat_reg_w = this.quat_reg_w;
imp.stereo_merge = this.stereo_merge;
imp.stereo_gap = this.stereo_gap;
imp.stereo_intereye = this.stereo_intereye;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 93c00b54
......@@ -5416,6 +5416,38 @@ public class OpticalFlow {
}
}
}
// later move to the right place
if ((quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
last_index //(quadCLTs.length -1) // int last_index,
);
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
double [] rms = new double[5];
double [] quat = new double[4];
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_index, // int last_index,
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
0); // int debugLevel
}
if (run_ly) {
if (debugLevel > -3) {
System.out.println("**** Running LY adjustments *****");
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 93c00b54
......@@ -221,6 +221,157 @@ public class QuadCLTCPU {
pimu_offsets = offsets;
}
// find best rotation between IMU XYZ and camera XYZ
/**
* Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS positions (currently
* only linear are used). Refined scene poses may be used as pull targets
* with free orientations (and angular velocities for ERS).
* Result poses are {0,0,0} for the reference frame.
*
* Assuming correct IMU angular velocities and offset linear ones.
*
* @param clt_parameters configuration parameters
* @param quadCLTs scenes sequence (for timestamps)
* @param xyzatr current scenes [[x,y,z],[a,t,r]] in reference scene frame
* @param ims_xyzatr IMU-derived (integrated) scene position and orientation in reference
* scene frame (only position is currently used)
* @param ref_index reference scene index
* @param early_index earliest (lowest) scene index to use
* @param last_index last (highest) scene index to use
* @return quaternion componets
*/
public static double [] getRotationFromXYZCameraIms(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] ims_xyzatr,
int ref_index,
int early_index,
int last_index,
double [] rms, // null or double[5];
int debugLevel
) {
double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene][0];
vect_y[nscene] = xyzatr[nscene][0];
}
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareLMA(
vect_x, // quat_lma_xyz, // double [][] vect_x,
vect_y, // double [][] vect_y,
null, // double [][] vect_w, all same weight
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debugLevel); // int debug_level)
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debugLevel); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (rms != null) { // null or double[2];
double [] last_rms = quaternionLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = quaternionLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
}
}
}
return quaternionLma.getQuaternion();
}
}
public static double [][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters,
QuadCLT[] quadCLTs,
double [][][] xyzatr,
double [][][] ims_xyzatr,
int ref_index,
int early_index,
int last_index,
double [] rms, // null or double[5];
double [] quaternion, // null or double[4]
int debugLevel
) {
double [] quat = getRotationFromXYZCameraIms(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
early_index, // int early_index,
last_index, // int last_index,
rms, // double [] rms, // null or double[5];
debugLevel); // int debugLevel
if (quat == null) {
return null;
}
if (quaternion != null) {
System.arraycopy(quat, 0, quaternion, 0, 4);
}
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS
double [] rotated_xyz = new double[3];
// double [] rotated_atr = new double[3];
for (int nscene = early_index; nscene <= last_index; nscene++) {
rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);
Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
Rotation rotation_atr = rot.applyTo(scene_atr);
rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
}
if (debugLevel > -1) {
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
"N","X","Y","Z","A","T","R",
"PIMU-X","PIMU-Y","PIMU-Z","PIMU-A","PIMU-T","PIMU-R",
"ROT-X","ROT-Y","ROT-Z","ROT-A","ROT-T","ROT-R"));
for (int nscene = early_index; nscene <= last_index; nscene++) {
System.out.println(String.format("%3s"+
"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+
"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
nscene,
xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],
xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],
ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],
ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],
rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],
rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));
}
}
return rotated_xyzatr;
}
/**
* Refine scene poses (now only linear) from currently adjusted poses
* (adjusted using IMS orientations) and IMS velocities (currently
......
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