Commit ef473c40 authored by Andrey Filippov's avatar Andrey Filippov

matched interscene pair

parent 468688b4
......@@ -13,6 +13,7 @@ import com.elphel.imagej.lwir.LwirReaderParameters;
import com.elphel.imagej.tileprocessor.BiQuadParameters;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.ImageDttParameters;
import com.elphel.imagej.tileprocessor.IntersceneLmaParameters;
import com.elphel.imagej.tileprocessor.MeasuredLayersFilterParameters;
import com.elphel.imagej.tileprocessor.OpticalFlowParameters;
import com.elphel.imagej.tileprocessor.PoleProcessorParameters;
......@@ -871,6 +872,7 @@ public class CLTParameters {
public MeasuredLayersFilterParameters mlfp = new MeasuredLayersFilterParameters();
public LwirReaderParameters lwir = new LwirReaderParameters();
public OpticalFlowParameters ofp = new OpticalFlowParameters();
public IntersceneLmaParameters ilp = new IntersceneLmaParameters();
public HashMap<String,Double> z_corr_map = new HashMap<String,Double>(); //old one
......@@ -1718,6 +1720,7 @@ public class CLTParameters {
poles.setProperties (prefix+"_poles", properties);
lwir.setProperties (prefix+"_lwir", properties);
ofp.setProperties (prefix+"_ofp_", properties);
ilp.setProperties (prefix+"_ilp_", properties);
}
......@@ -2540,6 +2543,7 @@ public class CLTParameters {
poles.getProperties (prefix+"_poles", properties);
lwir.getProperties (prefix+"_lwir", properties);
ofp.getProperties (prefix+"_ofp_", properties);
ilp.getProperties (prefix+"_ilp_", properties);
}
public boolean showJDialog() {
......@@ -3508,6 +3512,9 @@ public class CLTParameters {
gd.addTab ("O-Flow", "parameters for the interscene Optical FLow calculations");
this.ofp.dialogQuestions(gd);
gd.addTab ("Intra-LMA", "parameters for the interscene LMA fitting");
this.ilp.dialogQuestions(gd);
gd.addTab ("Debug", "Other debug images");
gd.addMessage ("--- Other debug images ---");
......@@ -4304,6 +4311,7 @@ public class CLTParameters {
this.lwir.dialogAnswers(gd);
this.ofp.dialogAnswers(gd);
this.ilp.dialogAnswers(gd);
this.debug_initial_discriminate= gd.getNextBoolean();
this.dbg_migrate= gd.getNextBoolean();
......
......@@ -700,6 +700,7 @@ private Panel panel1,
addButton("DSI histogram", panelClt5, color_report);
addButton("ML recalc", panelClt5, color_process);
addButton("Inter Test", panelClt5, color_stop);
addButton("Inter LMA", panelClt5, color_stop);
plugInFrame.add(panelClt5);
}
......@@ -5107,6 +5108,13 @@ private Panel panel1,
CLT_PARAMETERS.batch_run = true;
testInterScene();
return;
/* ======================================================================== */
} else if (label.equals("Inter LMA")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
CLT_PARAMETERS.batch_run = true;
testInterLMA();
return;
/* ======================================================================== */
} else if (label.equals("CLT rig edit")) {
......@@ -6475,6 +6483,75 @@ private Panel panel1,
}
public boolean testInterLMA() {
long startTime=System.nanoTime();
// load needed sensor and kernels files
if (!prepareRigImages()) return false;
String configPath=getSaveCongigPath();
if (configPath.equals("ABORT")) return false;
setAllProperties(PROPERTIES); // batchRig may save properties with the model. Extrinsics will be updated, others should be set here
if (DEBUG_LEVEL > -2){
System.out.println("++++++++++++++ Testing Interscene processing ++++++++++++++");
}
if (CLT_PARAMETERS.useGPU()) { // only init GPU instances if it is used
if (GPU_TILE_PROCESSOR == null) {
try {
GPU_TILE_PROCESSOR = new GPUTileProcessor(CORRECTION_PARAMETERS.tile_processor_gpu);
} catch (Exception e) {
System.out.println("Failed to initialize GPU class");
// TODO Auto-generated catch block
e.printStackTrace();
return false;
} //final int debugLevel);
}
if (CLT_PARAMETERS.useGPU(false) && (QUAD_CLT != null) && (GPU_QUAD == null)) { // if GPU main is needed
try {
GPU_QUAD = GPU_TILE_PROCESSOR.new GpuQuad(
QUAD_CLT,
4,
3);
} catch (Exception e) {
System.out.println("Failed to initialize GpuQuad class");
// TODO Auto-generated catch block
e.printStackTrace();
return false;
} //final int debugLevel);
QUAD_CLT.setGPU(GPU_QUAD);
}
}
try {
TWO_QUAD_CLT.TestInterLMA(
QUAD_CLT, // QuadCLT quadCLT_main,
// QUAD_CLT_AUX, // QuadCLT quadCLT_aux,
CLT_PARAMETERS, // EyesisCorrectionParameters.DCTParameters dct_parameters,
DEBAYER_PARAMETERS, //EyesisCorrectionParameters.DebayerParameters debayerParameters,
COLOR_PROC_PARAMETERS, //EyesisCorrectionParameters.ColorProcParameters colorProcParameters,
COLOR_PROC_PARAMETERS_AUX, //EyesisCorrectionParameters.ColorProcParameters colorProcParameters_aux,
CHANNEL_GAINS_PARAMETERS, //CorrectionColorProc.ColorGainsParameters channelGainParameters,
RGB_PARAMETERS, //EyesisCorrectionParameters.RGBParameters rgbParameters,
EQUIRECTANGULAR_PARAMETERS, // EyesisCorrectionParameters.EquirectangularParameters equirectangularParameters,
PROPERTIES, // Properties properties,
THREADS_MAX, //final int threadsMax, // maximal number of threads to launch
UPDATE_STATUS, //final boolean updateStatus,
DEBUG_LEVEL);
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
} //final int debugLevel);
if (configPath!=null) {
saveTimestampedProperties( // save config again
configPath, // full path or null
null, // use as default directory if path==null
true,
PROPERTIES);
}
System.out.println("batchRig(): Processing finished at "+
IJ.d2s(0.000000001*(System.nanoTime()-startTime),3)+" sec, --- Free memory="+
Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
return true;
}
......
......@@ -92,33 +92,33 @@ public class ErsCorrection extends GeometryCorrection {
static final int DW_DZ = 14; // dw_dz}; (m)
static final String [] DP_DERIV_NAMES = {
"dp_dpX", // (pix) 0
"dp_dpY", // (pix) 1
"dp_dd", // (pix) 2
"dp_dvaz", // (rad/sec) 3
"dp_dvtl", // (rad/sec) 4
"dp_dvrl", // (rad/sec) 5
"dp_dvx", // (m/s) 6
"dp_dvy", // (m/s) 7
"dp_dvz", // (m/s) 8
"dp_daz", // (rad) 9
"dp_dtl", // (rad) 10
"dp_drl", // (rad) 11
"dp_dx", // (m) 12
"dp_dy", // (m) 13
"dp_dz", // (m) 14
"dp_dsvaz", // (rad/sec)15
"dp_dsvtl", // (rad/sec)16
"dp_dsvrl", // (rad/sec)17
"dp_dsvx", // (m/s) 18
"dp_dsvy", // (m/s) 19
"dp_dsvz", // (m/s) 20
"dp_dsaz", // (rad) 21
"dp_dstl", // (rad) 22
"dp_dsrl", // (rad) 23
"dp_dsx", // (m) 24
"dp_dsy", // (m) 25
"dp_dsz"}; // (m) 26
"pX", // (pix) 0
"pY", // (pix) 1
"disp", // (pix) 2
"ers_vaz_ref", // (rad/sec) 3
"ers_vtl_ref", // (rad/sec) 4
"ers_vrl_ref", // (rad/sec) 5
"ers_dvx_ref", // (m/s) 6
"ers_dvy_ref", // (m/s) 7
"ers_dvz_ref", // (m/s) 8
"azimuth_ref", // (rad) 9
"tilt_ref", // (rad) 10
"roll_ref", // (rad) 11
"X_ref", // (m) 12
"Y_ref", // (m) 13
"Z_ref", // (m) 14
"ers_vaz_scene", // (rad/sec)15
"ers_vtl_scene", // (rad/sec)16
"ers_vrl_scene", // (rad/sec)17
"ers_dvx_scene", // (m/s) 18
"ers_dvy_scene", // (m/s) 19
"ers_dvz_scene", // (m/s) 20
"azimuth_scene", // (rad) 21
"tilt_scene", // (rad) 22
"Roll_scene", // (rad) 23
"X_scene", // (m) 24
"Y_scene", // (m) 25
"Z_scene"}; // (m) 26
// returned arrays have the zero element with coordinates, not derivatives
// Reference parameters
......@@ -150,6 +150,7 @@ public class ErsCorrection extends GeometryCorrection {
static final int DP_DSX = 24; // dw_dx, (m)
static final int DP_DSY = 25; // dw_dy, (m)
static final int DP_DSZ = 26; // dw_dz}; (m)
static final int DP_NUM_PARS = DP_DSZ+1;
static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
static final double THRESHOLD = 1E-10;
......@@ -922,7 +923,7 @@ public class ErsCorrection extends GeometryCorrection {
0.002}; // dw_dz}; (m) 14 was 0.1
double scale_delta = 1.0; // 0.1; // 1.0; // 0.1; // 0.5;
// double [] deltas = deltas0.clone();
double [] deltas = new double [DP_DSZ + 1];
double [] deltas = new double [DP_NUM_PARS];
System.arraycopy(deltas0, 0, deltas, 0, deltas0.length);
System.arraycopy(deltas0, 3, deltas, deltas0.length, deltas0.length - 3);
for (int i = 0; i < deltas.length; i++) deltas[i] *= scale_delta;
......@@ -2010,7 +2011,7 @@ public class ErsCorrection extends GeometryCorrection {
Matrix dpscene_dxyz = dx_dpscene.inverse();
Matrix dpscene_dxyz_minus = dpscene_dxyz.times(-1.0); // negated to calculate /d{pX,pY,D) for the scene parameters
double[][] derivatives = new double[DP_DSZ+2][]; // includes [0] - pXpYD vector
double[][] derivatives = new double[DP_NUM_PARS+1][]; // includes [0] - pXpYD vector
// scene pX, pY, Disparity
derivatives[0] = pXpYD_scene;
// derivatives by the reference parameters, starting with /dpX, /dpY, /dd
......@@ -2026,7 +2027,7 @@ public class ErsCorrection extends GeometryCorrection {
derivatives[vindx] = matrixTimesVector(dpscene_dxyz, reference_vectors[vindx]).toArray();
}
}
for (int indx = DP_DSVAZ; indx <= DP_DSZ; indx++) { // 15,16, ...
for (int indx = DP_DSVAZ; indx < DP_NUM_PARS; indx++) { // 15,16, ...
int indx_out = indx+1; // 16, 17,
int indx_in = indx_out - DP_DSVAZ + DW_DVAZ; // 4, 5, ...
if (is_infinity) {
......
......@@ -535,46 +535,6 @@ public class ExtrinsicAdjustment {
}
/*
private double [] getYminusFx(
GeometryCorrection.CorrVector corr_vector)
{
int clusters = clustersX * clustersY;
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE];
double [] imu = corr_vector.getIMU(); // i)
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivatives( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] DDNDderiv, // if not null, should be double[8][]
dy_ddisparity[cluster], // double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu, // double [] imu,
x0y0[cluster], // double [] pXYND0, // per-port non-distorted coordinates corresponding to the correlation measurements
world_xyz[cluster], // double [] xyz, // world XYZ for ERS correction
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
//arraycopy(Object src, int srcPos, Object dest, int destPos, int length)
ddnd[0] = -ddnd[0];
if ((force_disparity != null) && force_disparity[cluster]) {
ddnd[0] -= measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
}
/// ddnd[0] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] - ddnd[0];
for (int i = 0; i < NUM_SENSORS; i++) {
ddnd[i + 1] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + i] - ddnd[i + 1];
ddnd[i + 5] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + i] - ddnd[i + 5];
}
System.arraycopy(ddnd, 0, y_minus_fx, cluster * POINTS_SAMPLE, POINTS_SAMPLE);
}
return y_minus_fx;
}
*/
private double [] getWYmFxRms( // USED in lwir
double [] fx) {
int clusters = clustersX * clustersY;
......@@ -1125,7 +1085,7 @@ public class ExtrinsicAdjustment {
}
return clouds;
}
@Deprecated
private double [] getWeights(
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
......@@ -1395,13 +1355,7 @@ public class ExtrinsicAdjustment {
double [] imu = corr_vector.getIMU(); // i)
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE];
for (int cluster = 0; cluster < clusters; cluster++) {
// if ((cluster == 1892) || (cluster == 1894) ||(cluster == 3205)) {
// System.out.println("getFx() cluster="+cluster);
// }
if (measured_dsxy[cluster] != null){
// if ((cluster == 1735 ) || (cluster==1736)){
// System.out.print("");
// }
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivativesNew( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
......@@ -2220,13 +2174,8 @@ public class ExtrinsicAdjustment {
double lambda,
double rms_diff,
int debug_level) {
// int num_points = this.weights.length; // includes 2 extra for regularization
// int num_pars = getNumPars();
boolean [] rslt = {false,false};
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
// this.last_ymfx = getFxJt(
// this.vector, // double [] vector,
// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector);
if (debug_level > -1) { // temporary
......@@ -2304,19 +2253,10 @@ public class ExtrinsicAdjustment {
double [] delta = mdelta.getColumnPackedCopy();
GeometryCorrection.CorrVector corr_delta = geometryCorrection.getCorrVector(delta, par_mask);
/// double [] new_vector = this.vector.clone();
GeometryCorrection.CorrVector new_vector = this.corr_vector.clone();
double scale = 1.0;
// boolean ok =
new_vector.incrementVector(corr_delta, scale); // ok = false if there are nay NaN-s
/// for (int i = 0; i < num_pars; i++) new_vector[i]+= delta[i];
// being optimistic, modify jt and last_ymfx in place, restore if failed
/// this.last_ymfx = getFxJt(
/// new_vector, // double [] vector,
/// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(new_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(new_vector);
if (debug_level > 2) {
......@@ -2339,18 +2279,12 @@ public class ExtrinsicAdjustment {
if (debug_level > 2) {
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
/// for (int np = 0; np < vector.length; np++) {
/// System.out.print(this.vector[np]+" ");
/// }
System.out.println();
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
/// this.last_ymfx = getFxJt( // recalculate fx
/// this.vector, // double [] vector,
/// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector);
......
package com.elphel.imagej.tileprocessor;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAdder;
import Jama.Matrix;
public class IntersceneLma {
OpticalFlow opticalFlow = null;
QuadCLT [] scenesCLT = null; // now will use just 2 - 0 -reference scene, 1 - scene.
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
private double [] y_vector = null; // sum of fx(initial parameters) and correlation offsets
// private double [][] vector_XYS = null; // optical flow X,Y, confidence obtained from the correlate2DIterate()
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private boolean [] par_mask = null;
private int [] par_indices = null;
private double [] backup_parameters_full = null; // indices match DP_DPX...DP_DSZ (first 3 are not used)
private double [] parameters_vector = null;
private double [] parameters_initial = null; // (will be used to pull for regularization)
private double [][] macrotile_centers = null; // (will be used to pull for regularization)
private double infinity_disparity = 0.1; // treat lower as infinity
private int num_samples = 0;
public IntersceneLma(
OpticalFlow opticalFlow
) {
this.opticalFlow = opticalFlow;
}
public double [] getSceneXYZ() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSX],full_vector[ErsCorrection.DP_DSY],full_vector[ErsCorrection.DP_DSZ]};
}
public double [] getSceneATR() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]};
}
public double [] getReferenceXYZ() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DX],full_vector[ErsCorrection.DP_DY],full_vector[ErsCorrection.DP_DZ]};
}
public double [] getReferenceATR() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]};
}
public double [] getSceneERSXYZ() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]};
}
public double [] getSceneERSATR() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]};
}
public double [] getReferenceERSXYZ() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]};
}
public double [] getReferenceERSATR() {
double [] full_vector = getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]};
}
public void prepareLMA(
final double [] scene_xyz0, // camera center in world coordinates (or null to use instance)
final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
// reference atr, xyz are considered 0.0
final QuadCLT scene_QuadClt,
final QuadCLT reference_QuadClt,
final boolean[] param_select,
final double [] param_regweights,
final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
final double [][] centers, // macrotile centers (in pixels and average disparities
final int debug_level)
{
scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
// this.vector_XYS = vector_XYS;
par_mask = param_select;
macrotile_centers = centers;
num_samples = 2 * centers.length;
ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
final double [] scene_xyz = (scene_xyz0 != null) ? scene_xyz0 : ers_scene.camera_xyz;
final double [] scene_atr = (scene_atr0 != null) ? scene_atr0 : ers_scene.camera_atr;
final double [] reference_xyz = new double[3];
final double [] reference_atr = new double[3];
double [] full_parameters_vector = new double [] {
0.0, 0.0, 0.0,
ers_ref.ers_watr_center_dt[0], ers_ref.ers_watr_center_dt[1], ers_ref.ers_watr_center_dt[2],
ers_ref.ers_wxyz_center_dt[0], ers_ref.ers_wxyz_center_dt[1], ers_ref.ers_wxyz_center_dt[2],
reference_atr[0], reference_atr[1], reference_atr[2],
reference_xyz[0], reference_xyz[1], reference_xyz[2],
ers_scene.ers_watr_center_dt[0], ers_scene.ers_watr_center_dt[1], ers_scene.ers_watr_center_dt[2],
ers_scene.ers_wxyz_center_dt[0], ers_scene.ers_wxyz_center_dt[1], ers_scene.ers_wxyz_center_dt[2],
scene_atr[0], scene_atr[1], scene_atr[2],
scene_xyz[0], scene_xyz[1], scene_xyz[2]};
backup_parameters_full = full_parameters_vector.clone();
int num_pars = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) num_pars++;
par_indices = new int [num_pars];
num_pars = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) par_indices[num_pars++] = i;
parameters_vector = new double [par_indices.length];
for (int i = 0; i < par_indices.length; i++) parameters_vector[i] = full_parameters_vector[par_indices[i]];
parameters_initial = parameters_vector.clone();
setSamplesWeights(vector_XYS); // not regularization yet !
last_jt = new double [parameters_vector.length][];
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
scenesCLT[1], // final QuadCLT scene_QuadClt,
scenesCLT[0], // final QuadCLT reference_QuadClt,
debug_level); // final int debug_level)
double [][] wjtj = getWJtJlambda( // USED in lwir
0.0, // final double lambda,
last_jt); // final double [][] jt)
for (int i = 0; i < parameters_vector.length; i++) {
int indx = num_samples + i;
weights[indx] = param_regweights[par_indices[i]]/Math.sqrt(wjtj[i][i]);
}
normalizeWeights(); // make full weight == 1.0; pure_weight <= 1.0;
// remeasure fx - now with regularization terms.
fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
scene_QuadClt, // final QuadCLT scene_QuadClt,
reference_QuadClt, // final QuadCLT reference_QuadClt,
debug_level); // final int debug_level)
y_vector = fx.clone();
for (int i = 0; i < vector_XYS.length; i++) {
if (vector_XYS[i] != null){
y_vector[2 * i + 0] += vector_XYS[i][0];
y_vector[2 * i + 1] += vector_XYS[i][1];
}
}
last_rms = new double [2];
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
initial_rms = last_rms.clone();
good_or_bad_rms = this.last_rms.clone();
}
public boolean runLma(
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
int debug_level)
{
boolean [] rslt = {false,false};
this.last_rms = null; // remove?
int iter = 0;
for (iter = 0; iter < num_iter; iter++) {
rslt = lmaStep(
lambda,
rms_diff,
debug_level);
if (rslt == null) {
return false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (rslt[1]) {
break;
}
if (rslt[0]) { // good
lambda *= lambda_scale_good;
} else {
lambda *= lambda_scale_bad;
if (lambda > lambda_max) {
break; // not used in lwir
}
}
}
if (rslt[0]) { // better
if (iter >= num_iter) { // better, but num tries exceeded
if (debug_level > 0) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
} else {
if (debug_level > 0) System.out.println("Step "+iter+": LMA: Success");
}
} else { // improved over initial ?
if (last_rms[0] < initial_rms[0]) {
rslt[0] = true;
if (debug_level > 0) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
} else {
if (debug_level > 0) System.out.println("Step "+iter+": Failed to converge");
}
}
if (debug_level > 0) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
return rslt[0];
}
private boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
boolean [] rslt = {false,false};
// maybe the following if() branch is not needed - already done in prepareLMA !
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
last_rms = new double[2];
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
scenesCLT[1], // final QuadCLT scene_QuadClt,
scenesCLT[0], // final QuadCLT reference_QuadClt,
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
if (debug_level > -1) { // temporary
/*
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
// TODO: Restore/implement
if (debug_level > 3) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary
this.last_jt)); // double [][] jt)
if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv = null;
try {
jtjl_inv = wjtjlambda.inverse(); // check for errors
} catch (RuntimeException e) {
rslt[1] = true;
if (debug_level > 0) {
System.out.println("Singular Matrix!");
}
return rslt;
}
if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
//last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) {
System.out.println("mdelta");
mdelta.print(18, 6);
}
double scale = 1.0;
double [] delta = mdelta.getColumnPackedCopy();
double [] new_vector = parameters_vector.clone();
for (int i = 0; i < parameters_vector.length; i++) {
new_vector[i] += scale * delta[i];
}
double [] fx = getFxDerivs(
new_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
scenesCLT[1], // final QuadCLT scene_QuadClt,
scenesCLT[0], // final QuadCLT reference_QuadClt,
debug_level); // final int debug_level)
double [] rms = new double[2];
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
rms); // final double [] rms_fp // null or [2]
if (debug_level > 2) {
/*
dbgYminusFx(this.last_ymfx, "next y-fX");
dbgXY(new_vector, "XY-correction");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
this.parameters_vector = new_vector.clone();
if (debug_level > 2) {
// print vectors in some format
/*
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
System.out.println();
*/
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
scenesCLT[1], // final QuadCLT scene_QuadClt,
scenesCLT[0], // final QuadCLT reference_QuadClt,
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
this.last_rms); // final double [] rms_fp // null or [2]
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
if (debug_level > 2) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
return rslt;
}
private void setSamplesWeights(
final double [][] vector_XYS) // not regularization yet
{
this.weights = new double [num_samples + parameters_vector.length];
final Thread[] threads = ImageDtt.newThreadArray(opticalFlow.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final DoubleAdder asum_weight = new DoubleAdder();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < vector_XYS.length; iMTile = ai.getAndIncrement()) if (vector_XYS[iMTile] != null){
double w = vector_XYS[iMTile][2];
weights[2 * iMTile] = w;
asum_weight.add(w);
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
final double s = 0.5/asum_weight.sum();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < vector_XYS.length; iMTile = ai.getAndIncrement()) if (vector_XYS[iMTile] != null){
weights[2 * iMTile] *= s;
weights[2 * iMTile + 1] = weights[2 * iMTile];
}
}
};
}
ImageDtt.startAndJoin(threads);
pure_weight = 1.0;
}
private void normalizeWeights()
{
//num_samples
final Thread[] threads = ImageDtt.newThreadArray(opticalFlow.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final DoubleAdder asum_weight = new DoubleAdder();
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int i = ai.getAndIncrement(); i < num_samples; i = ai.getAndIncrement()){
asum_weight.add(weights[i]);
}
}
};
}
ImageDtt.startAndJoin(threads);
final double s_pure = asum_weight.sum();
for (int i = 0; i < par_indices.length; i++) {
int indx = num_samples + i;
asum_weight.add(weights[indx]);
}
double full_weight = asum_weight.sum();
pure_weight = s_pure/full_weight;
final double s = 1.0/asum_weight.sum();
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int i = ai.getAndIncrement(); i < weights.length; i = ai.getAndIncrement()){
weights[i] *= s;
}
}
};
}
ImageDtt.startAndJoin(threads);
}
private double [] getFullVector(double [] vector) {
double [] full_vector = backup_parameters_full.clone();
for (int i = 0; i < par_indices.length; i++) {
full_vector[par_indices[i]] = vector[i];
}
return full_vector;
}
private double [] getFxDerivs(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final QuadCLT scene_QuadClt,
final QuadCLT reference_QuadClt,
final int debug_level)
{
final double [] full_vector = getFullVector(vector);
final ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
final ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
final double [] scene_xyz = new double[3];;
final double [] scene_atr = new double[3];
final double [] reference_xyz = new double[3]; // will stay 0
final double [] reference_atr = new double[3]; // will stay 0
final double [] fx = new double [weights.length];
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
jt[i] = new double [weights.length];
}
}
for (int i = 0; i <3; i++) {
ers_ref.ers_watr_center_dt[i] = full_vector[ErsCorrection.DP_DVAZ + i];
ers_ref.ers_wxyz_center_dt[i] = full_vector[ErsCorrection.DP_DVX + i];
ers_scene.ers_watr_center_dt[i] = full_vector[ErsCorrection.DP_DSVAZ + i];
ers_scene.ers_wxyz_center_dt[i] = full_vector[ErsCorrection.DP_DSVX + i];
reference_atr[i] = full_vector[ErsCorrection.DP_DAZ + i];
reference_xyz[i] = full_vector[ErsCorrection.DP_DX + i];
scene_atr[i] = full_vector[ErsCorrection.DP_DSAZ + i];
scene_xyz[i] = full_vector[ErsCorrection.DP_DSX + i];
}
ers_scene.setupERS();
ers_ref.setupERS();
final Matrix [] reference_matrices_inverse = ErsCorrection.getInterRotDeriveMatrices(
reference_atr, // double [] atr);
true); // boolean invert));
final Matrix [] scene_matrices_inverse = ErsCorrection.getInterRotDeriveMatrices(
scene_atr, // double [] atr);
true); // boolean invert));
final Matrix scene_rot_matrix = ErsCorrection.getInterRotDeriveMatrices(
scene_atr, // double [] atr);
false)[0]; // boolean invert));
// double [][][] derivs = new double [macrotile_centers.length + parameters_vector.length][][];
final Thread[] threads = ImageDtt.newThreadArray(opticalFlow.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < macrotile_centers.length; iMTile = ai.getAndIncrement()) {
if ((macrotile_centers[iMTile]!=null) &&(weights[iMTile] > 0.0)){
//infinity_disparity
boolean is_infinity = macrotile_centers[iMTile][2] < infinity_disparity;
double [][] deriv_params = ers_ref.getDPxSceneDParameters(
ers_scene, // ErsCorrection ers_scene,
true, // boolean correctDistortions,
is_infinity, // boolean is_infinity,
macrotile_centers[iMTile], // double [] pXpYD_reference,
reference_xyz, // double [] reference_xyz, // reference (this) camera center in world coordinates
scene_xyz, // double [] scene_xyz, // (other, ers_scene) camera center in world coordinates
reference_matrices_inverse, // Matrix [] reference_matrices_inverse,
scene_matrices_inverse, // Matrix [] scene_matrices_inverse,
scene_rot_matrix, // Matrix scene_rot_matrix, // single rotation (direct) matrix for the scene
debug_level); // int debug_level);
if (deriv_params!= null) {
fx[2 * iMTile + 0] = deriv_params[0][0]; // pX
fx[2 * iMTile + 1] = deriv_params[0][1]; // pY
if (jt != null) {
for (int i = 0; i < par_indices.length; i++) {
int indx = par_indices[i] + 1;
jt[i][2 * iMTile + 0] = deriv_params[indx][0]; // pX
jt[i][2 * iMTile + 1] = deriv_params[indx][1]; // pY (disparity is not used)
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// pull to the initial parameter values
for (int i = 0; i < par_indices.length; i++) {
fx [i + 2 * macrotile_centers.length] = vector[i]; // - parameters_initial[i]; // scale will be combined with weights
jt[i][i + 2 * macrotile_centers.length] = 1.0; // scale will be combined with weights
}
return fx;
}
private double [][] getWJtJlambda( // USED in lwir
final double lambda,
final double [][] jt)
{
final int num_pars = jt.length;
final int num_pars2 = num_pars * num_pars;
final int nup_points = jt[0].length;
final double [][] wjtjl = new double [num_pars][num_pars];
final Thread[] threads = ImageDtt.newThreadArray(opticalFlow.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
int i = indx / num_pars;
int j = indx % num_pars;
if (j >= i) {
double d = 0.0;
for (int k = 0; k < nup_points; k++) {
d += weights[k]*jt[i][k]*jt[j][k];
}
wjtjl[i][j] = d;
if (i == j) {
wjtjl[i][j] += d * lambda;
} else {
wjtjl[j][i] = d;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return wjtjl;
}
private double [] getYminusFxWeighted(
// final double [][] vector_XYS,
final double [] fx,
final double [] rms_fp // null or [2]
) {
final Thread[] threads = ImageDtt.newThreadArray(opticalFlow.threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final DoubleAdder asum_weight = new DoubleAdder();
final double [] wymfw = new double [fx.length];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int i = ai.getAndIncrement(); i < num_samples; i = ai.getAndIncrement()) {
double d = y_vector[i] - fx[i];
double wd = d * weights[i];
double l2 = d * wd;
wymfw[i] = wd;
asum_weight.add(l2);
}
}
};
}
ImageDtt.startAndJoin(threads);
double s_rms = asum_weight.sum();
double rms_pure = Math.sqrt(s_rms/pure_weight);
for (int i = 0; i < par_indices.length; i++) {
int indx = i + num_samples;
// double d = parameters_initial[i] - fx[indx]; // fx[indx] == vector[i]
double d = y_vector[indx] - fx[indx]; // fx[indx] == vector[i]
double wd = d * weights[indx];
s_rms += d * wd;
wymfw[indx] = wd;
}
double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0; /pure_weight); shey should be re-normalized after adding regularization
if (rms_fp != null) {
rms_fp[0] = rms;
rms_fp[1] = rms_pure;
}
return wymfw;
}
/*
* par_indices
double [] rslt = {rms, rms_pure};
*
vector_XYS *
private double [] getFx(
GeometryCorrection.CorrVector corr_vector)
{
int clusters = clustersX * clustersY;
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i)
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE];
for (int cluster = 0; cluster < clusters; cluster++) {
if (measured_dsxy[cluster] != null){
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivativesNew( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] DDNDderiv, // if not null, should be double[8][]
pY_offset[cluster], // double [] py_offset, // array of per-port average pY offset from the center (to correct ERS) or null (for no ERS)
imu, // double [] imu,
x0y0[cluster], // double [] pXYND0, // per-port non-distorted coordinates corresponding to the correlation measurements
world_xyz[cluster], // double [] xyz, // world XYZ for ERS correction
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
//arraycopy(Object src, int srcPos, Object dest, int destPos, int length)
// System.arraycopy(src_pixels, 0, dst_pixels, 0, src_pixels.length); for the borders closer to 1/2 kernel size
ddnd[0] = ddnd[0]; // ?
for (int i = 0; i < NUM_SENSORS; i++) {
ddnd[i + 1] = ddnd[i + 1];
ddnd[i + 5] = ddnd[i + 5];
}
System.arraycopy(ddnd, 0, y_minus_fx, cluster*POINTS_SAMPLE, POINTS_SAMPLE);
}
}
return y_minus_fx;
}
private double [][] getJacobianTransposed(
GeometryCorrection.CorrVector corr_vector,
double delta){
int clusters = clustersX * clustersY;
int num_pars = getNumPars();
double [][] jt = new double [num_pars][clusters * POINTS_SAMPLE ];
double rdelta = 1.0/delta;
for (int par = 0; par < num_pars; par++) {
double [] pars = new double[num_pars];
pars[par] = delta;
GeometryCorrection.CorrVector corr_delta = geometryCorrection.getCorrVector(pars, par_mask);
GeometryCorrection.CorrVector corr_vectorp = corr_vector.clone();
GeometryCorrection.CorrVector corr_vectorm = corr_vector.clone();
corr_vectorp.incrementVector(corr_delta, 0.5);
corr_vectorm.incrementVector(corr_delta, -0.5);
double [] fx_p = getFx(corr_vectorp);
double [] fx_m = getFx(corr_vectorm);
for (int i = 0; i < fx_p.length; i++) {
jt[par][i] = (fx_p[i] - fx_m[i])*rdelta;
}
}
return jt;
}
private int [] setWeights( // number right, number left
double [][] measured_dsxy,
boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
boolean [] filtered_infinity, // tiles known to be infinity
double [] distance_from_edge,// to reduce weight of the mountain ridge, increase clouds (or null)
int min_num_forced, // if number of forced samples exceeds this, zero out weights of non-forced
boolean infinity_right_left, // each halve should have > min_num_forced, will calculate separate average
double weight_infinity, // total weight of infinity tiles fraction (0.0 - 1.0)
double weight_disparity, // disparity weight relative to the sum of 8 lazy eye values of the same tile
double weight_disparity_inf, // disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
double max_disparity_far, // reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
double max_disparity_use) // do not process near objects to avoid ERS
{}
*/
}
package com.elphel.imagej.tileprocessor;
import java.util.Properties;
import com.elphel.imagej.common.GenericJTabbedDialog;
public class IntersceneLmaParameters {
public boolean [] ilma_lma_select = new boolean [ErsCorrection.DP_NUM_PARS]; // first three will not be used
public double [] ilma_regularization_weights = new double [ErsCorrection.DP_NUM_PARS]; // first three will not be used
public double ilma_lambda = 0.1;
public double ilma_lambda_scale_good = 0.5;
public double ilma_lambda_scale_bad = 8.0;
public double ilma_lambda_max = 100;
public double ilma_rms_diff = 0.001;
public int ilma_num_iter = 20;
public int ilma_debug_level = 1;
public IntersceneLmaParameters() {
ilma_lma_select[ErsCorrection.DP_DSAZ]= true;
ilma_lma_select[ErsCorrection.DP_DSTL]= true;
ilma_lma_select[ErsCorrection.DP_DSRL]= true;
ilma_lma_select[ErsCorrection.DP_DSX]= true;
ilma_lma_select[ErsCorrection.DP_DSY]= true;
ilma_lma_select[ErsCorrection.DP_DSZ]= true;
}
public void dialogQuestions(GenericJTabbedDialog gd) {
gd.addMessage("Interframe LMA parameters selection");
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
gd.addCheckbox (ErsCorrection.DP_DERIV_NAMES[i], this.ilma_lma_select[i],
"Adjust parameter "+ErsCorrection.DP_DERIV_NAMES[i]+" with interscene LMA" );
}
gd.addMessage("Regularization parameters - pull strength to the initial values");
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
gd.addNumericField(ErsCorrection.DP_DERIV_NAMES[i], this.ilma_regularization_weights[i], 6,8,"",
"Weight of "+ErsCorrection.DP_DERIV_NAMES[i]+" pull, 1.0 means that the paramter offset from initial corresponding to 1 image pixel\n"+
" will cause error equal to all reprojection ones");
}
gd.addMessage("LMA other parameters");
gd.addNumericField("LMA lambda", this.ilma_lambda, 6,8,"",
"Initial value of the LMA lambda");
gd.addNumericField("Scale lambda after successful LMA iteration", this.ilma_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.ilma_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.ilma_lambda_max, 2,7,"",
"Fail LMA if the result is still worse than before parameters were updates.");
gd.addNumericField("Minimal relative RMSE improvement", this.ilma_rms_diff, 5,7,"",
"Exit LMA iterations if relative RMSE improvement drops below this value.");
gd.addNumericField("Maximal number of LMA iterations", this.ilma_num_iter, 0,3,"",
"A hard limit on LMA iterations.");
gd.addNumericField("Debug level", this.ilma_debug_level, 0,3,"",
"Debug level of interscene LMA operation.");
}
public void dialogAnswers(GenericJTabbedDialog gd) {
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
this.ilma_lma_select[i] = gd.getNextBoolean();
}
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
this.ilma_regularization_weights[i] = gd.getNextNumber();
}
this.ilma_lambda = gd.getNextNumber();
this.ilma_lambda_scale_good = gd.getNextNumber();
this.ilma_lambda_scale_bad = gd.getNextNumber();
this.ilma_lambda_max = gd.getNextNumber();
this.ilma_rms_diff = gd.getNextNumber();
this.ilma_num_iter = (int) gd.getNextNumber();
this.ilma_debug_level = (int) gd.getNextNumber();
}
public void setProperties(String prefix,Properties properties){
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
properties.setProperty(prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_sel", this.ilma_lma_select[i]+"");
properties.setProperty(prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_regweight", this.ilma_regularization_weights[i]+"");
}
properties.setProperty(prefix+"ilma_lambda", this.ilma_lambda+"");
properties.setProperty(prefix+"ilma_lambda_scale_good", this.ilma_lambda_scale_good+"");
properties.setProperty(prefix+"ilma_lambda_scale_bad", this.ilma_lambda_scale_bad+"");
properties.setProperty(prefix+"ilma_lambda_max", this.ilma_lambda_max+"");
properties.setProperty(prefix+"ilma_rms_diff", this.ilma_rms_diff+"");
properties.setProperty(prefix+"ilma_num_iter", this.ilma_num_iter+"");
properties.setProperty(prefix+"ilma_debug_level", this.ilma_debug_level+"");
}
public void getProperties(String prefix,Properties properties){
for (int i = ErsCorrection.DP_DVAZ; i < ErsCorrection.DP_NUM_PARS; i++) {
String pn_sel = prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_sel";
if (properties.getProperty(pn_sel)!=null) this.ilma_lma_select[i]=Boolean.parseBoolean(properties.getProperty(pn_sel));
pn_sel = prefix+ErsCorrection.DP_DERIV_NAMES[i]+"_regweight";
if (properties.getProperty(pn_sel)!=null) this.ilma_regularization_weights[i]=Double.parseDouble(properties.getProperty(pn_sel));
}
if (properties.getProperty(prefix+"ilma_lambda")!=null) this.ilma_lambda=Double.parseDouble(properties.getProperty(prefix+"ilma_lambda"));
if (properties.getProperty(prefix+"ilma_lambda_scale_good")!=null) this.ilma_lambda_scale_good=Double.parseDouble(properties.getProperty(prefix+"ilma_lambda_scale_good"));
if (properties.getProperty(prefix+"ilma_lambda_scale_bad")!=null) this.ilma_lambda_scale_bad=Double.parseDouble(properties.getProperty(prefix+"ilma_lambda_scale_bad"));
if (properties.getProperty(prefix+"ilma_lambda_max")!=null) this.ilma_lambda_max=Double.parseDouble(properties.getProperty(prefix+"ilma_lambda_max"));
if (properties.getProperty(prefix+"ilma_rms_diff")!=null) this.ilma_rms_diff=Double.parseDouble(properties.getProperty(prefix+"ilma_rms_diff"));
if (properties.getProperty(prefix+"ilma_num_iter")!=null) this.ilma_num_iter=Integer.parseInt(properties.getProperty(prefix+"ilma_num_iter"));
if (properties.getProperty(prefix+"ilma_debug_level")!=null) this.ilma_debug_level=Integer.parseInt(properties.getProperty(prefix+"ilma_debug_level"));
}
@Override
public IntersceneLmaParameters clone() throws CloneNotSupportedException {
IntersceneLmaParameters ilp = new IntersceneLmaParameters();
System.arraycopy(this.ilma_lma_select, 0, ilp.ilma_lma_select, 0, ilma_lma_select.length);
System.arraycopy(this.ilma_regularization_weights, 0, ilp.ilma_regularization_weights, 0, ilma_regularization_weights.length);
ilp.ilma_lambda = this.ilma_lambda;
ilp.ilma_lambda_scale_good = this.ilma_lambda_scale_good;
ilp.ilma_lambda_scale_bad = this.ilma_lambda_scale_bad;
ilp.ilma_lambda_max = this.ilma_lambda_max;
ilp.ilma_rms_diff = this.ilma_rms_diff;
ilp.ilma_num_iter = this.ilma_num_iter;
ilp.ilma_debug_level = this.ilma_debug_level;
return ilp;
}
}
......@@ -296,6 +296,51 @@ public class OpticalFlow {
return flowXYS;
}
private void removeOutliers(
double nsigma,
double [][] flowXYS)
{
double swx = 0.0, swy = 0.0, sw = 0.0;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null) {
double w = flowXYS[i][2];
swx += flowXYS[i][0]* w;
swy += flowXYS[i][1]* w;
sw += w;
}
if (sw > 0.0) {
swx /= sw;
swy /= sw;
// calculate deviation regardless of weight
int n = 0;
double s2 = 0.0;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null) {
double dx = flowXYS[i][0] - swx;
double dy = flowXYS[i][1] - swy;
s2 += dx*dx+dy*dy;
n++;
}
s2/= n;
n=0;
double s2_max = s2 * nsigma * nsigma;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null) {
double dx = flowXYS[i][0] - swx;
double dy = flowXYS[i][1] - swy;
if ((dx*dx+dy*dy) > s2_max) {
flowXYS[i] = null;
n++;
}
}
System.out.println("Removed "+n+" outliers");
double err = 4.0;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null){
flowXYS[i][0] += err;
flowXYS[i][1] += err;
}
}
}
/**
* Show a 2/3-slice image for the optical flow (only X,Y or X,Y,Confidence)
* @param title image title (null or empty - will not show)
......@@ -309,12 +354,15 @@ public class OpticalFlow {
{
if ((title != null) && !title.equals("")) {
int height = flowXYS.length/width;
String [] titles0 ={"dX","dY","Strength"};
int nslices = titles0.length;
// String [] titles0 ={"dX","dY","Strength"};
String [] titles ={"dX","dY","Strength","Err","dX-weighted","dY-weighted","Werr"};
// int nslices = titles0.length;
int nslices = 0;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null) {
nslices = flowXYS[i].length;
break;
}
/*
if (nslices > titles0.length) {
nslices = titles0.length;
}
......@@ -323,14 +371,26 @@ public class OpticalFlow {
for (int i = 0; i < nslices; i++) {
titles[i] = titles0[i];
}
*/
final double [][] dbg_img = new double [titles.length][width * height];
for (int l = 0; l < dbg_img.length; l++) {
Arrays.fill(dbg_img[l], Double.NaN);
}
for (int mtile = 0; mtile < flowXYS.length; mtile++) if (flowXYS[mtile] != null){
for (int l = 0; l < dbg_img.length; l++) {
// for (int l = 0; l < dbg_img.length; l++) {
for (int l = 0; l < nslices; l++) {
dbg_img[l][mtile] = flowXYS[mtile][l];
}
if (nslices > 2) {
dbg_img[3][mtile] = Math.sqrt(
flowXYS[mtile][0] * flowXYS[mtile][0] +
flowXYS[mtile][1] * flowXYS[mtile][1]);
dbg_img[4][mtile] = flowXYS[mtile][0] * flowXYS[mtile][2];
dbg_img[5][mtile] = flowXYS[mtile][1] * flowXYS[mtile][2];
dbg_img[6][mtile] = Math.sqrt(
flowXYS[mtile][0] * flowXYS[mtile][0] +
flowXYS[mtile][1] * flowXYS[mtile][1]) * flowXYS[mtile][2];
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
......@@ -402,6 +462,7 @@ public class OpticalFlow {
* @param scene_atr Scene azimuth, tilt and roll (or null to use scene instance).
* @param scene_QuadClt Scene QuadCLT instance.
* @param reference_QuadClt Reference QuadCLT instance.
* @param reference_tiles_macro
* @param reference_center_occupancy Fraction of non-null tiles in the center 8x8 area of the reference macrotiles after disparity
* filtering (see tolerance_absolute, tolerance_relative). Below this threshold - skip that macrotile.
* @param flowXY0 Initial offset of scene tiles (in image pixels) in x (right) and y (down) directions or null (to use all zeros)
......@@ -442,6 +503,7 @@ public class OpticalFlow {
final double [] scene_atr, // camera orientation relative to world frame
final QuadCLT scene_QuadClt,
final QuadCLT reference_QuadClt,
final double [][] reference_tiles_macro,
final double reference_center_occupancy, // fraction of remaining tiles in the center 8x8 area (<1.0)
// final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
// flowXY should be initialized to all pairs of zeros (or deliberate pixel offset pairs if initial error is too high, will be modified with each iteration
......@@ -488,6 +550,15 @@ public class OpticalFlow {
num_laplassian, // final int num_passes,
change_laplassian, // final double max_change,
-1); //-1); // 2); // final int debug_level)
if (reference_tiles_macro != null) {
double [][] macro_centers = getMacroPxPyDisp(
reference_QuadClt, // final QuadCLT reference_QuadClt,
reference_tiles //final double [][][] reference_tiles // prepared with prepareReferenceTiles() + fillTilesNans();
);
for (int i = 0; i < reference_tiles_macro.length; i++) {
reference_tiles_macro[i] = macro_centers[i];
}
}
final double [][] flowXY = (flowXY0 == null) ? (new double [reference_tiles.length][2]):flowXY0;
final double [][] flowXY_frac = new double [reference_tiles.length][]; // Will contain fractional X/Y shift for CLT
......@@ -1120,6 +1191,72 @@ public class OpticalFlow {
return corr_tiles;
}
/**
* Get width of the macrotiles array
* @param reference_QuadClt scene instance
* @return width of a macrotile array
*/
public int getMacroWidth(final QuadCLT reference_QuadClt) {
final TileProcessor tp = reference_QuadClt.getTileProcessor();
return tp.getTilesX()/tp.getTileSize();
}
/**
* Get triplets of {pX, pY, disparity} for each reference macrotile to use with LMA fitting
* @param reference_QuadClt reference scene instance
* @param reference_tiles reference tiles prepared with prepareReferenceTiles()
* @return Array of [macrotile]{pX, pY, disparity}, some macrotiles may be null
*/
public double [][] getMacroPxPyDisp(
final QuadCLT reference_QuadClt,
final double [][][] reference_tiles // prepared with prepareReferenceTiles() + fillTilesNans();
)
{
final int margin = 0; // 1; // extra margins over 16x16 tiles to accommodate distorted destination tiles
final TileProcessor tp = reference_QuadClt.getTileProcessor();
final int tilesX = tp.getTilesX();
final int tilesY = tp.getTilesY();
final int transform_size = tp.getTileSize();
final int macroTilesX = tilesX/transform_size;
final int macroTilesY = tilesY/transform_size;
final int macroX0 = (tilesX - macroTilesX * transform_size)/2; // distribute extra tiles symmetrically ==0 for 324
final int macroY0 = (tilesY - macroTilesY * transform_size)/2; // distribute extra tiles symmetrically (242 - 1 tile above and below)
final double [][] pXpYD = new double [macroTilesX*macroTilesY][];
final int fullTileSize = 2 * (transform_size + margin);
final int fullTileLen = fullTileSize * fullTileSize;
final Thread[] threads = ImageDtt.newThreadArray(threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < pXpYD.length; iMTile = ai.getAndIncrement()) {
if (reference_tiles[iMTile] != null) {
int mtileY = iMTile / macroTilesX;
int mtileX = iMTile % macroTilesX;
double pY = transform_size * (mtileY * transform_size + macroY0 + transform_size/2);
double pX = transform_size * (mtileX * transform_size + macroX0 + transform_size/2);
// find average disparity
double sw = 0.0, swd = 0.0;
for (int iTile = 0; iTile < fullTileLen; iTile++) {
double disparity = reference_tiles[iMTile][QuadCLT.DSRBG_DISPARITY][iTile];
double strength = reference_tiles[iMTile][QuadCLT.DSRBG_STRENGTH][iTile];
if (!Double.isNaN(disparity) && (strength> 0.0)) {
sw += strength;
swd += strength * disparity;
}
}
if (sw > 0.0) {
pXpYD[iMTile] = new double[] {pX, pY, swd/sw};
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return pXpYD;
}
/**
* Prepare scene tiles for correlation with the reference ones. Tiles include 5 layers: disparity,
* strength and 3 average color components (red, blue and green).
......@@ -1568,13 +1705,19 @@ public class OpticalFlow {
// show debug image
String title = scene_QuadClt.getImageName()+"-scene_tiles";
String title = reference_QuadClt.getImageName() + "-" + scene_QuadClt.getImageName()+"ref-scene";
/*
showMacroTiles(
title, // String title,
scene_tiles, // double [][][] source_tiles,
scene_QuadClt, // final QuadCLT qthis,
margin); // final int margin); // extra margins over 16x16 tiles to accommodate distorted destination tiles
*/
showComareMacroTiles(
title, // String title,
new double [][][][] {reference_tiles, scene_tiles}, // double [][][][] source_tiles_sets,
scene_QuadClt, // final QuadCLT qthis,
margin); // final int margin); // extra margins over 16x16 tiles to accommodate distorted destination tiles
String [] dbg_titles= {"dX","dY"};
String dbg_title= "flowXY_frac"; // TODO: Visualize RMS of individual tiles fitting
......@@ -1979,7 +2122,9 @@ public class OpticalFlow {
}
ErsCorrection ersReferenceCorrection = reference_QuadClt.getErsCorrection();
ErsCorrection ersSceneCorrection = scene_QuadClt.getErsCorrection();
ersReferenceCorrection.setupERS(); // just in case - setUP using instance paRAMETERS
ersSceneCorrection.setupERS();
double [] zbuffer = new double [tiles];
for (int tileY = 0; tileY < tilesY; tileY++) {
for (int tileX = 0; tileX < tilesX; tileX++) {
......@@ -1992,6 +2137,7 @@ public class OpticalFlow {
}
// found that there are tiles with strength == 0.0, while disparity is not NaN
if (!Double.isNaN(disparity) && (dsrbg_camera[QuadCLT.DSRBG_STRENGTH][nTile] > 0.0)) {
/*
double [] pXpYD = ersReferenceCorrection.getImageCoordinatesReferenceERS( // ersCorrection - reference
scene_QuadClt, // QuadCLT cameraQuadCLT, // camera station that got image to be to be matched
......@@ -2005,6 +2151,21 @@ public class OpticalFlow {
scene_xyz, // double [] camera_xyz, // camera center in world coordinates
scene_atr, // double [] camera_atr, // camera orientation relative to world frame
LINE_ERR); // double line_err) // threshold error in scan lines (1.0)
*/
// swapping reference <-> scene
double [] pXpYD = ersSceneCorrection.getImageCoordinatesERS( // ersCorrection - reference
reference_QuadClt, // QuadCLT cameraQuadCLT, // camera station that got image to be to be matched
centerX, // double px, // pixel coordinate X in the reference view
centerY, // double py, // pixel coordinate Y in the reference view
disparity, // double disparity, // reference disparity
true, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
scene_xyz, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
scene_atr, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
true, // boolean distortedCamera, // camera view is distorted (false - rectilinear)
ZERO3, // double [] camera_xyz, // camera center in world coordinates
ZERO3, // double [] camera_atr, // camera orientation relative to world frame
LINE_ERR); // double line_err) // threshold error in scan lines (1.0)
if (pXpYD != null) {
int px = (int) Math.round(pXpYD[0]/transform_size);
int py = (int) Math.round(pXpYD[1]/transform_size);
......@@ -2081,6 +2242,202 @@ public class OpticalFlow {
return dsrbg_out;
}
public double[][][] test_LMA(
CLTParameters clt_parameters,
double k_prev,
QuadCLT reference_QuadCLT,
QuadCLT scene_QuadCLT,
double corr_scale, // = 0.75
int debug_level)
{
TileProcessor tp = reference_QuadCLT.getTileProcessor();
final int iscale = 8;
double ts = reference_QuadCLT.getTimeStamp();
double ts_prev = ts;
double [] camera_xyz0 = ZERO3.clone();
double [] camera_atr0 = ZERO3.clone();
ErsCorrection ersCorrection = reference_QuadCLT.getErsCorrection();
String this_image_name = reference_QuadCLT.getImageName();
System.out.println("\n"+this_image_name+":\n"+ersCorrection.extrinsic_corr.toString());
System.out.println(String.format("%s: ers_wxyz_center= %f, %f, %f", this_image_name,
ersCorrection.ers_wxyz_center[0], ersCorrection.ers_wxyz_center[1],ersCorrection.ers_wxyz_center[2] ));
System.out.println(String.format("%s: ers_wxyz_center_dt= %f, %f, %f", this_image_name,
ersCorrection.ers_wxyz_center_dt[0], ersCorrection.ers_wxyz_center_dt[1],ersCorrection.ers_wxyz_center_dt[2] ));
System.out.println(String.format("%s: ers_wxyz_center_d2t= %f, %f, %f", this_image_name,
ersCorrection.ers_wxyz_center_d2t[0], ersCorrection.ers_wxyz_center_d2t[1],ersCorrection.ers_wxyz_center_d2t[2] ));
System.out.println(String.format("%s: ers_watr_center_dt= %f, %f, %f", this_image_name,
ersCorrection.ers_watr_center_dt[0], ersCorrection.ers_watr_center_dt[1],ersCorrection.ers_watr_center_dt[2] ));
System.out.println(String.format("%s: ers_watr_center_d2t= %f, %f, %f", this_image_name,
ersCorrection.ers_watr_center_d2t[0], ersCorrection.ers_watr_center_d2t[1],ersCorrection.ers_watr_center_d2t[2] ));
double dt = 0.0;
if (scene_QuadCLT == null) {
scene_QuadCLT = reference_QuadCLT;
}
if (scene_QuadCLT != null) {
ts_prev = scene_QuadCLT.getTimeStamp();
dt = ts-ts_prev;
if (dt < 0) {
k_prev = (1.0-k_prev);
}
if (Math.abs(dt) > 0.15) { // at least two frames TODO: use number of lines* line_time * ...?
k_prev = 0.5;
System.out.println("Non-consecutive frames, dt = "+dt);
}
ErsCorrection ersCorrectionPrev = (ErsCorrection) (scene_QuadCLT.geometryCorrection);
double [] wxyz_center_dt_prev = ersCorrectionPrev.ers_wxyz_center_dt;
double [] watr_center_dt_prev = ersCorrectionPrev.ers_watr_center_dt;
double [] wxyz_delta = new double[3];
double [] watr_delta = new double[3];
for (int i = 0; i <3; i++) {
wxyz_delta[i] = corr_scale * dt * (k_prev * wxyz_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_wxyz_center_dt[i]);
watr_delta[i] = corr_scale * dt * (k_prev * watr_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_watr_center_dt[i]);
}
camera_xyz0 = wxyz_delta;
camera_atr0 = watr_delta;
}
int tilesX = tp.getTilesX();
int tilesY = tp.getTilesY();
String [] dsrbg_titles = {"d", "s", "r", "b", "g"};
int num_lma = 5;
for (int nlma = 0; nlma < num_lma; nlma++) {
// debugging mismatch in transformCameraVew compared to tile view
reference_QuadCLT.getErsCorrection().setupERS();// just in case - setup using instance parameters
scene_QuadCLT.getErsCorrection().setupERS();// just in case - setup using instance parameters
// above did not help
double [][] dsrbg = transformCameraVew( // shifts previous image correctly (right)
camera_xyz0, // double [] camera_xyz, // camera center in world coordinates
camera_atr0, //double [] camera_atr, // camera orientation relative to world frame
scene_QuadCLT, // QuadCLT camera_QuadClt,
reference_QuadCLT, // reference
iscale);
double [][] dsrbg_ref = transformCameraVew( // shifts previous image correctly (right)
ZERO3, // camera_xyz0, // double [] camera_xyz, // camera center in world coordinates
ZERO3, // camera_atr0, // double [] camera_atr, // camera orientation relative to world frame
reference_QuadCLT, // scene_QuadCLT, // QuadCLT camera_QuadClt,
reference_QuadCLT, // reference_QuadCLT, // reference
iscale);
// double [][][] pair = {reference_QuadCLT.getDSRBG(),dsrbg};
double [][][] pair = {dsrbg_ref, dsrbg};
// combine this scene with warped previous one
if (debug_level > -2) {
String [] rtitles = new String[2* dsrbg_titles.length];
double [][] dbg_rslt = new double [rtitles.length][];
for (int i = 0; i < dsrbg_titles.length; i++) {
rtitles[2*i] = dsrbg_titles[i]+"0";
rtitles[2*i+1] = dsrbg_titles[i];
dbg_rslt[2*i] = pair[0][i];
dbg_rslt[2*i+1] = pair[1][i];
}
String title = this_image_name+"-"+scene_QuadCLT.image_name+"-dt"+dt;
(new ShowDoubleFloatArrays()).showArrays(
dbg_rslt,
tilesX,
tilesY,
true,
title,
rtitles);
}
// Add limitations on disparity ? To be used with multi-tile consolidation
// Check with walking, not only rotating
int transform_size = tp.getTileSize();
int macroTilesX = tilesX/transform_size;
int macroTilesY = tilesY/transform_size;
int macroTiles = macroTilesX * macroTilesY;
double [][] flowXY = new double [macroTiles][2]; // zero pre-shifts
// double [][] flowXY_frac = new double [macroTiles][]; // Will contain fractional X/Y shift for CLT
double [][] reference_tiles_macro = new double [macroTiles][];
double [][] vector_XYS = correlate2DIterate( // returns optical flow and confidence
// for prepareSceneTiles()
camera_xyz0, // final double [] scene_xyz, // camera center in world coordinates
camera_atr0, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
reference_tiles_macro, // final double [][] reference_tiles_macro,
clt_parameters.ofp.center_occupancy_ref, // final double reference_center_occupancy, // fraction of remaining tiles in the center 8x8 area (<1.0)
// flowXY should be initialized to all pairs of zeros (or deliberate pixel offset pairs if initial error is too high, will be modified with each iteration
flowXY, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions // initialize to [reference_tiles.length][2]
clt_parameters.ofp.tolerance_absolute_inter, // final double tolerance_absolute, // absolute disparity half-range in each tile
clt_parameters.ofp.tolerance_relative_inter, // final double tolerance_relative, // relative disparity half-range in each tile
clt_parameters.ofp.occupancy_inter, // final double occupancy, // fraction of remaining tiles (<1.0)
clt_parameters.ofp.num_laplassian, // final int num_passes,
clt_parameters.ofp.change_laplassian, // final double max_change,
// for correlate2DSceneToReference ()
clt_parameters.ofp.chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
clt_parameters.ofp.corr_sigma, // final double corr_sigma,
clt_parameters.ofp.fat_zero, // final double fat_zero,
clt_parameters.ofp.late_normalize_iterate, // final boolean late_normalize,
// for correlation2DToVectors_CM()
clt_parameters.ofp.iradius_cm, // final int iradius, // half-size of the square to process
clt_parameters.ofp.dradius_cm, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
clt_parameters.ofp.refine_num_cm, // final int refine_num, // number of iterations to apply weights around new center
clt_parameters.ofp.num_refine_all, // final int num_run_all, // run all tiles for few iterations before filtering
clt_parameters.ofp.max_refines, // final int max_tries,
// for recalculateFlowXY()
clt_parameters.ofp.magic_scale, // final double magic_scale, // 0.85 for CM
clt_parameters.ofp.min_change, // final double min_change,
clt_parameters.ofp.best_neibs_num, // final int best_num,
clt_parameters.ofp.ref_stdev, // final double ref_stdev,
clt_parameters.ofp.debug_level_iterate); // final int debug_level)
if (debug_level > -2) {
String dbg_title = "OpticalFlow-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
showVectorXYConfidence(
dbg_title, // String title,
vector_XYS, // double [][] flowXYS,
macroTilesX); // int width)
}
double nsigma = 1.7;
removeOutliers(
nsigma, // double nsigma,
vector_XYS); // double [][] flowXYS)
if (debug_level > -2) {
String dbg_title = "OpticalFlowFiltered-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
showVectorXYConfidence(
dbg_title, // String title,
vector_XYS, // double [][] flowXYS,
macroTilesX); // int width)
}
IntersceneLma intersceneLma = new IntersceneLma(
this); // OpticalFlow opticalFlow
intersceneLma.prepareLMA(
camera_xyz0, // final double [] scene_xyz0, // camera center in world coordinates (or null to use instance)
camera_atr0, // final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
// reference atr, xyz are considered 0.0
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, //final QuadCLT reference_QuadClt,
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
vector_XYS, // final double [][] vector_XYS, // optical flow X,Y, confidence obtained from the correlate2DIterate()
reference_tiles_macro, // final double [][] centers, // macrotile centers (in pixels and average disparities
debug_level); // final int debug_level)
boolean lmaOK = intersceneLma.runLma(
clt_parameters.ilp.ilma_lambda, // double lambda, // 0.1
clt_parameters.ilp.ilma_lambda_scale_good, // double lambda_scale_good,// 0.5
clt_parameters.ilp.ilma_lambda_scale_bad, // double lambda_scale_bad, // 8.0
clt_parameters.ilp.ilma_lambda_max, // double lambda_max, // 100
clt_parameters.ilp.ilma_rms_diff, // double rms_diff, // 0.001
clt_parameters.ilp.ilma_num_iter, // int num_iter, // 20
clt_parameters.ilp.ilma_debug_level); // int debug_level)
if (!lmaOK) {
System.out.println("LMA failed");
break;
}
camera_xyz0 = intersceneLma.getSceneXYZ();
camera_atr0 = intersceneLma.getSceneATR();
}
return null; //pair;
}
/**
* A top-level method for testing optical flow generating, currently includes temporary testing functionality
......@@ -2256,6 +2613,7 @@ public class OpticalFlow {
camera_atr0, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
null, // final double [][] reference_tiles_macro,
clt_parameters.ofp.center_occupancy_ref, // final double reference_center_occupancy, // fraction of remaining tiles in the center 8x8 area (<1.0)
// flowXY should be initialized to all pairs of zeros (or deliberate pixel offset pairs if initial error is too high, will be modified with each iteration
flowXY, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions // initialize to [reference_tiles.length][2]
......
......@@ -8279,6 +8279,86 @@ if (debugLevel > -100) return true; // temporarily !
System.out.println("End of test");
}
public void TestInterLMA(
QuadCLT quadCLT_main, // tiles should be set
CLTParameters clt_parameters,
EyesisCorrectionParameters.DebayerParameters debayerParameters,
ColorProcParameters colorProcParameters,
ColorProcParameters colorProcParameters_aux,
CorrectionColorProc.ColorGainsParameters channelGainParameters,
EyesisCorrectionParameters.RGBParameters rgbParameters,
EyesisCorrectionParameters.EquirectangularParameters equirectangularParameters,
Properties properties,
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel) throws Exception
{
if ((quadCLT_main != null) && (quadCLT_main.getGPU() != null)) {
quadCLT_main.getGPU().resetGeometryCorrection();
quadCLT_main.gpuResetCorrVector(); // .getGPU().resetGeometryCorrectionVector();
}
// final boolean batch_mode = clt_parameters.batch_run;
this.startTime=System.nanoTime();
String [] sourceFiles0=quadCLT_main.correctionsParameters.getSourcePaths();
QuadCLT.SetChannels [] set_channels_main = quadCLT_main.setChannels(debugLevel);
if ((set_channels_main == null) || (set_channels_main.length==0)) {
System.out.println("No files to process (of "+sourceFiles0.length+")");
return;
}
QuadCLT.SetChannels [] set_channels=quadCLT_main.setChannels(debugLevel);
// String set_name = set_channels[0].set_name;
QuadCLT [] quadCLTs = new QuadCLT [set_channels.length];
for (int i = 0; i < quadCLTs.length; i++) {
quadCLTs[i] = quadCLT_main.spawnQuadCLT(
set_channels[i].set_name,
clt_parameters,
colorProcParameters, //
threadsMax,
debugLevel);
// temporarily fix wrong sign:
ErsCorrection ers = (ErsCorrection) (quadCLTs[i].getGeometryCorrection());
ers.setupERSfromExtrinsics();
quadCLTs[i].setDSRBG(
clt_parameters, // CLTParameters clt_parameters,
threadsMax, // int threadsMax, // maximal number of threads to launch
updateStatus, // boolean updateStatus,
debugLevel); // int debugLevel)
/// quadCLTs[i].showDSIMain();
}
OpticalFlow opticalFlow = new OpticalFlow(
threadsMax, // int threadsMax, // maximal number of threads to launch
updateStatus); // boolean updateStatus);
for (int i = 1; i < quadCLTs.length; i++) {
QuadCLT qPrev = (i > 0) ? quadCLTs[i - 1] : null;
// double [][][] pair_sets =
/*
opticalFlow.test_LMA(
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.ofp.k_prev, // k_prev,
quadCLTs[i],
qPrev,
clt_parameters.ofp.ers_to_pose_scale, // corr_scale,
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
*/
opticalFlow.test_LMA(
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.ofp.k_prev, // k_prev,
qPrev,
quadCLTs[i],
clt_parameters.ofp.ers_to_pose_scale, // corr_scale,
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
}
System.out.println("End of test");
}
public void batchLwirRig(
......
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