Skip to content

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

tested fitting convergence on long image sets

parent ef473c40
Show whitespace changes
Inline Side-by-side
Showing with 1150 additions and 193 deletions
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ 3f43f2ac
......@@ -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 Pairs", panelClt5, color_process);
addButton("Inter LMA", panelClt5, color_stop);
plugInFrame.add(panelClt5);
}
......@@ -5108,6 +5109,13 @@ private Panel panel1,
CLT_PARAMETERS.batch_run = true;
testInterScene();
return;
/* ======================================================================== */
} else if (label.equals("Inter Pairs")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
CLT_PARAMETERS.batch_run = true;
interPairsLMA();
return;
/* ======================================================================== */
} else if (label.equals("Inter LMA")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
......@@ -5116,6 +5124,7 @@ private Panel panel1,
testInterLMA();
return;
/* ======================================================================== */
} else if (label.equals("CLT rig edit")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
......@@ -6533,6 +6542,79 @@ private Panel panel1,
RGB_PARAMETERS, //EyesisCorrectionParameters.RGBParameters rgbParameters,
EQUIRECTANGULAR_PARAMETERS, // EyesisCorrectionParameters.EquirectangularParameters equirectangularParameters,
PROPERTIES, // Properties properties,
true, // false, // boolean reset_from_extrinsics,
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;
}
public boolean interPairsLMA() {
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.interPairsLMA(
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,
true, // false, // boolean reset_from_extrinsics,
THREADS_MAX, //final int threadsMax, // maximal number of threads to launch
UPDATE_STATUS, //final boolean updateStatus,
DEBUG_LEVEL);
......
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @ 3f43f2ac
......@@ -120,6 +120,35 @@ public class ErsCorrection extends GeometryCorrection {
"Y_scene", // (m) 25
"Z_scene"}; // (m) 26
static final String [] DP_VECTORS_NAMES = {
"pXpYD", // (pix) 0
null, // (pix) 1
null, // (pix) 2
"ERS_VATR_REF", // (rad/sec) 3
null, // (rad/sec) 4
null, // (rad/sec) 5
"ERS_VXYZ_REF", // (m/s) 6
null, // (m/s) 7
null, // (m/s) 8
"ATR_REF", // (rad) 9
null, // (rad) 10
null, // (rad) 11
"XYZ_REF", // (m) 12
null, // (m) 13
null, // (m) 14
"ERS_VATR_SCENE", // (rad/sec)15
null, // (rad/sec)16
null, // (rad/sec)17
"ERS_VXYZ_SCENE", // (m/s) 18
null, // (m/s) 19
null, // (m/s) 20
"ATR_SCENE", // (rad) 21
null, // (rad) 22
null, // (rad) 23
"XYZ_SCENE", // (m) 24
null, // (m) 25
null}; // (m) 26
// returned arrays have the zero element with coordinates, not derivatives
// Reference parameters
static final int DP_DPX = 0; // dw_dpX, (pix)
......@@ -177,7 +206,7 @@ public class ErsCorrection extends GeometryCorrection {
private Quaternion[] ers_quaternion; // per scan line
private Quaternion[] ers_quaternion_dt; // per scan line
private double [][] ers_atr; // azimuth-tilt-roll per scan line
private double [][] ers_atr_dt; // angular velocities per scan line
private double [][] ers_atr_dt; // angular velocities per scan line. It is now actually 2*omega!
public void setPose(
double [] camera_xyz,
......@@ -191,6 +220,21 @@ public class ErsCorrection extends GeometryCorrection {
public double [] getCameraATR() {
return camera_atr;
}
public double [] getErsXYZ_dt() {
return ers_wxyz_center_dt;
}
public double [] getErsATR_dt() {
return ers_watr_center_dt;
}
public double [] getErsXYZ_d2t() {
return ers_wxyz_center_d2t;
}
public double [] getErsATR_d2t() {
return ers_watr_center_d2t;
}
public void setPropertiesPose(String prefix, Properties properties){
properties.setProperty(prefix+XYZ_PREFIX, String.format("%f, %f, %f", camera_xyz[0], camera_xyz[1], camera_xyz[2]));
......@@ -214,9 +258,9 @@ public class ErsCorrection extends GeometryCorrection {
public boolean getPropertiesERS(String prefix,Properties properties){
boolean got_data = false;
if (properties.getProperty(prefix+ERS_XYZ_PREFIX)!=null) {ers_wxyz_center = parseDoublesCSV(properties.getProperty(prefix+XYZ_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_XYZ_DT_PREFIX)!=null) {ers_wxyz_center_dt = parseDoublesCSV(properties.getProperty(prefix+ATR_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_XYZ_D2T_PREFIX)!=null) {ers_wxyz_center_d2t = parseDoublesCSV(properties.getProperty(prefix+ATR_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_XYZ_PREFIX)!=null) {ers_wxyz_center = parseDoublesCSV(properties.getProperty(prefix+ERS_XYZ_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_XYZ_DT_PREFIX)!=null) {ers_wxyz_center_dt = parseDoublesCSV(properties.getProperty(prefix+ERS_XYZ_DT_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_XYZ_D2T_PREFIX)!=null) {ers_wxyz_center_d2t = parseDoublesCSV(properties.getProperty(prefix+ERS_XYZ_D2T_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_ATR_DT_PREFIX)!=null) {ers_watr_center_dt = parseDoublesCSV(properties.getProperty(prefix+ERS_ATR_DT_PREFIX)); got_data=true;}
if (properties.getProperty(prefix+ERS_ATR_D2T_PREFIX)!=null) {ers_watr_center_d2t = parseDoublesCSV(properties.getProperty(prefix+ERS_ATR_D2T_PREFIX)); got_data=true;}
if (got_data) {
......@@ -227,7 +271,11 @@ public class ErsCorrection extends GeometryCorrection {
public void setPropertiesScenes(String prefix, Properties properties){
String [] timestamps = getScenes();
for (String k : timestamps) {
properties.setProperty(prefix+SCENES_PREFIX+"_"+k, getScene(k).toString());
String [] s_scenes = getScene(k).toStrings();
properties.setProperty(prefix+SCENES_PREFIX+"_"+k, s_scenes[0]);
properties.setProperty(prefix+SCENES_PREFIX+"_"+k+"_dt", s_scenes[1]);
properties.setProperty(prefix+SCENES_PREFIX+"_"+k+"_d2t", s_scenes[2]);
// properties.setProperty(prefix+SCENES_PREFIX+"_"+k, getScene(k).toString());
}
}
......@@ -245,7 +293,11 @@ public class ErsCorrection extends GeometryCorrection {
if (!timestamps.isEmpty()) {
got_data = true;
for (String ts:timestamps) {
addScene(ts, new XyzAtr(properties.getProperty(prefix+ts)));
String pose = properties.getProperty(prefix+ts);
String ers_dt = properties.getProperty(prefix+ts+"_dt");
String ers_d2t = properties.getProperty(prefix+ts+"_d2t");
// addScene(ts, new XyzAtr(properties.getProperty(prefix+ts)));
addScene(ts, new XyzAtr(new String [] {pose, ers_dt, ers_d2t}));
}
}
return got_data;
......@@ -289,6 +341,14 @@ public class ErsCorrection extends GeometryCorrection {
scenes_poses.put(timestamp, new XyzAtr(xyz, atr));
}
public void addScene(String timestamp, double [] xyz, double [] atr, double [] ers_xyz_dt, double [] ers_atr_dt) {
scenes_poses.put(timestamp, new XyzAtr(xyz, atr, ers_xyz_dt, ers_atr_dt));
}
public void addScene(String timestamp, double [] xyz, double [] atr, double [] ers_xyz_dt, double [] ers_atr_dt, double [] ers_xyz_d2t, double [] ers_atr_d2t) {
scenes_poses.put(timestamp, new XyzAtr(xyz, atr, ers_xyz_dt, ers_atr_dt, ers_xyz_d2t, ers_atr_d2t));
}
public XyzAtr getScene(String timestamp) { // null if not found
return scenes_poses.get(timestamp);
}
......@@ -303,6 +363,26 @@ public class ErsCorrection extends GeometryCorrection {
if (scene == null) return null;
return scene.getATR();
}
public double[] getSceneErsXYZ_dt(String timestamp) {
XyzAtr scene = scenes_poses.get(timestamp);
if (scene == null) return null;
return scene.getErsXYZ_dt();
}
public double[] getSceneErsATR_dt(String timestamp) {
XyzAtr scene = scenes_poses.get(timestamp);
if (scene == null) return null;
return scene.getErsATR_dt();
}
public double[] getSceneErsXYZ_d2t(String timestamp) {
XyzAtr scene = scenes_poses.get(timestamp);
if (scene == null) return null;
return scene.getErsXYZ_d2t();
}
public double[] getSceneErsATR_d2t(String timestamp) {
XyzAtr scene = scenes_poses.get(timestamp);
if (scene == null) return null;
return scene.getErsATR_d2t();
}
......@@ -313,6 +393,12 @@ public class ErsCorrection extends GeometryCorrection {
public class XyzAtr {
double [] xyz;
double [] atr;
double [] ers_xyz_dt; // world camera Vx, Vy, Vz (m/s)
double [] ers_atr_dt; // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
double [] ers_atr_d2t; // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
double [] ers_xyz_d2t; // world camera Vx, Vy, Vz (m/s^2)
public XyzAtr() {
xyz = new double[3];
atr = new double[3];
......@@ -321,17 +407,88 @@ public class ErsCorrection extends GeometryCorrection {
public XyzAtr(double [] xyz, double [] atr) {
this.xyz = xyz;
this.atr = atr;
this.ers_xyz_dt = new double[3];
this.ers_atr_dt = new double[3];
this.ers_xyz_d2t = new double[3];
this.ers_atr_d2t = new double[3];
}
public XyzAtr(
double [] xyz,
double [] atr,
double [] ers_xyz_dt,
double [] ers_atr_dt) {
this.xyz = xyz;
this.atr = atr;
this.ers_xyz_dt = ers_xyz_dt;
this.ers_atr_dt = ers_xyz_dt;
this.ers_xyz_d2t = new double[3];
this.ers_atr_d2t = new double[3];
}
public XyzAtr(
double [] xyz,
double [] atr,
double [] ers_xyz_dt,
double [] ers_atr_dt,
double [] ers_xyz_d2t,
double [] ers_atr_d2t
) {
this.xyz = xyz;
this.atr = atr;
this.ers_xyz_dt = ers_xyz_dt;
this.ers_atr_dt = ers_xyz_dt;
this.ers_xyz_d2t = ers_xyz_d2t;
this.ers_atr_d2t = ers_xyz_d2t;
}
public XyzAtr(String s) {
double [] d = parseDoublesCSV(s);
xyz = new double [] {d[0], d[1], d[2]};
atr = new double [] {d[3], d[4], d[5]};
}
public XyzAtr(String [] ss) {
if (ss[0] != null) {
double [] d = parseDoublesCSV(ss[0]);
xyz = new double [] {d[0], d[1], d[2]};
atr = new double [] {d[3], d[4], d[5]};
this.ers_xyz_dt = new double[3];
this.ers_atr_dt = new double[3];
this.ers_xyz_d2t = new double[3];
this.ers_atr_d2t = new double[3];
if (ss.length > 1) {
if (ss[1] != null) {
d = parseDoublesCSV(ss[1]);
ers_xyz_dt = new double [] {d[0], d[1], d[2]};
ers_atr_dt = new double [] {d[3], d[4], d[5]};
}
if (ss.length > 2) {
if (ss[2] != null) {
d = parseDoublesCSV(ss[2]);
ers_xyz_d2t = new double [] {d[0], d[1], d[2]};
ers_atr_d2t = new double [] {d[3], d[4], d[5]};
}
}
}
}
}
public String toString() {
return String.format("%f, %f, %f, %f, %f, %f",xyz[0],xyz[1],xyz[2],atr[0],atr[1],atr[2]);
}
public String [] toStrings() {
return new String[] {
String.format("%f, %f, %f, %f, %f, %f",xyz[0], xyz[1], xyz[2], atr[0], atr[1], atr[2]),
String.format("%f, %f, %f, %f, %f, %f",ers_xyz_dt[0], ers_xyz_dt[1], ers_xyz_dt[2], ers_atr_dt[0], ers_atr_dt[1], ers_atr_dt[2]),
String.format("%f, %f, %f, %f, %f, %f",ers_xyz_d2t[0], ers_xyz_d2t[1], ers_xyz_d2t[2], ers_atr_d2t[0], ers_atr_d2t[1], ers_atr_d2t[2])};
}
public double [] getXYZ() {
return xyz;
}
......@@ -349,6 +506,41 @@ public class ErsCorrection extends GeometryCorrection {
atr[2] = d[2];
}
public double [] getErsXYZ_dt() {
return ers_xyz_dt;
}
public double [] getErsATR_dt() {
return ers_atr_dt;
}
public double [] getErsXYZ_d2t() {
return ers_xyz_d2t;
}
public double [] getErsATR_d2t() {
return ers_atr_d2t;
}
public void setErsXYZ_dt(double [] d) {
ers_xyz_dt[0] = d[0];
ers_xyz_dt[1] = d[1];
ers_xyz_dt[2] = d[2];
}
public void setErsATR_dt(double [] d) {
ers_atr_dt[0] = d[0];
ers_atr_dt[1] = d[1];
ers_atr_dt[2] = d[2];
}
public void setErsXYZ_d2t(double [] d) {
ers_xyz_d2t[0] = d[0];
ers_xyz_d2t[1] = d[1];
ers_xyz_d2t[2] = d[2];
}
public void setErsATR_d2t(double [] d) {
ers_atr_d2t[0] = d[0];
ers_atr_d2t[1] = d[1];
ers_atr_d2t[2] = d[2];
}
}
/**
......@@ -419,7 +611,7 @@ public class ErsCorrection extends GeometryCorrection {
// use deep=true for the independent instance (clone), false - to "upgrade" GeometryCorrection to ErsCorrection
public ErsCorrection(GeometryCorrection gc, boolean deep) {
debugLevel = gc.debugLevel;
line_time = gc.line_time; // 26.5E-6; // duration of sensor scan line (for ERS)
line_time = gc.line_time; // 36.38! //26.5E-6; // duration of sensor scan line (for ERS)
pixelCorrectionWidth= gc.pixelCorrectionWidth; // 2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2)
pixelCorrectionHeight= gc.pixelCorrectionHeight; // 1936;
focalLength = gc.focalLength; // =FOCAL_LENGTH;
......@@ -534,15 +726,21 @@ public class ErsCorrection extends GeometryCorrection {
// setup from extrinsics vector
public void setupERSfromExtrinsics()
{
double BUGS = 26.5/36.38; // tempoorary correction for the wrong scan line time!
double [] ersv = getCorrVector().getIMU();
setupERS(
new double [3], // double [] wxyz_center, // world camera XYZ (meters) for the frame center
new double [] {ersv[3], ersv[4], ersv[5]}, // double [] wxyz_center_dt, // world camera Vx, Vy, Vz (m/s)
new double [] {BUGS*ersv[3], BUGS*ersv[4], BUGS*ersv[5]}, // double [] wxyz_center_dt, // world camera Vx, Vy, Vz (m/s)
new double [3], // double [] wxyz_center_d2t, // world camera Vx, Vy, Vz (m/s^2)
// double [] watr_center, // camera orientation (az, tilt, roll in radians, corresponding to the frame center)
// new double [] {ersv[1], ersv[0], ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
// REVERSING tilt sign !
new double [] {ersv[1], -ersv[0], ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
/// new double [] {ersv[1], -ersv[0], ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
//// new double [] {-ersv[1], -ersv[0], ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
// 2* - because watr_center_dt is now 2 * omega!
// new double [] {-2*BUGS*ersv[1], -2*BUGS*ersv[0], 2*BUGS*ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
new double [] {-2*BUGS*ersv[1], -2*BUGS*ersv[0], -2*BUGS*ersv[2]}, // double [] watr_center_dt, // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
new double [3]); // double [] watr_center_d2t) // camera rotaions (az, tilt, roll in radians/s, corresponding to the frame center)
}
......@@ -730,64 +928,6 @@ public class ErsCorrection extends GeometryCorrection {
return pXpYD;
}
public double [] getImageCoordinatesReferenceERS(
QuadCLT cameraQuadCLT, // camera station that got image to be to be matched
double px, // pixel coordinate X in this camera view
double py, // pixel coordinate Y in this camera view
double disparity, // this view disparity
boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
double [] reference_xyz, // this view position in world coordinates (typically zero3)
double [] reference_atr, // this view orientation relative to world frame (typically zero3)
boolean distortedCamera, // camera view is distorted (false - rectilinear)
double [] camera_xyz, // camera center in world coordinates
double [] camera_atr, // camera orientation relative to world frame
double line_err) // threshold error in scan lines (1.0)
{
if (reference_xyz == null) reference_xyz = this.camera_xyz;
if (reference_atr == null) reference_atr = this.camera_atr;
ErsCorrection ers_camera = this;
if (cameraQuadCLT != null) {
ers_camera = cameraQuadCLT.getErsCorrection();
}
if (camera_xyz == null) camera_xyz = ers_camera.camera_xyz;
if (camera_atr == null) camera_atr = ers_camera.camera_atr;
// Find world coordinates of the camera pixel
double [] xyzw = ers_camera.getWorldCoordinatesERS( // {x - left,y - up, z (0 at camera, negative away), 1} for real, {x,y,z,0} - for infinity
px,
py,
disparity,
distortedView, // correct distortion (will need corrected background too !)
camera_xyz, // camera center in world coordinates
camera_atr); // camera orientation relative to world frame
if (xyzw == null) {
return null;
}
if (xyzw[3] == 0.0) { // infinity
/*
if (xyzw[2] > 0) {
for (int i = 0; i < 3; i++) {
xyzw[i] = -xyzw[i];
}
}
*/
}
if (xyzw[2] > 0) {
return null; // can not match object behind the camera
}
double [] pXpYD = ers_camera.getImageCoordinatesERS( // USED in lwir
xyzw,
distortedCamera,
reference_xyz, // camera center in world coordinates
reference_atr, // camera orientation relative to world frame
line_err); // threshold error in scan lines (1.0)
return pXpYD;
}
/**
* Get real world coordinates from pixel coordinates and nominal disparity
* @param px horizontal pixel coordinate (right)
......@@ -1297,7 +1437,8 @@ public class ErsCorrection extends GeometryCorrection {
}
if (nTile == dbg_tile) {
System.out.println("comparePXYD_Derivatives(): nTile = "+nTile+" ("+tileX+"/"+tileY+"), is_infinity="+is_infinity[nTile]);
double [][] deriv_params = getDPxSceneDParameters(
// double [][] deriv_params =
getDPxSceneDParameters(
scene_ers, // ErsCorrection ers_scene,
true, // boolean correctDistortions,
!is_infinity [nTile], // boolean is_infinity,
......
src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java
View file @ 3f43f2ac
......@@ -57,7 +57,7 @@ public class GeometryCorrection {
"velocity_x", "velocity_y", "velocity_z"};
public int debugLevel = 0;
public double line_time = 26.5E-6; // duration of sensor scan line (for ERS)
public double line_time = 26.5E-6; // duration of sensor scan line (for ERS) Wrong, 36.38us (change and re-run ERS
public int pixelCorrectionWidth=2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2)
public int pixelCorrectionHeight=1936;
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneLma.java
View file @ 3f43f2ac
package com.elphel.imagej.tileprocessor;
import java.util.ArrayList;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAdder;
......@@ -19,9 +20,10 @@ public class IntersceneLma {
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_full = null; // full parameters vector for the currenl LMA run
private double [] backup_parameters_full = null; // full parameters vector before first LMA run
private double [] parameters_vector = null;
private double [] parameters_initial = null; // (will be used to pull for regularization)
// 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;
......@@ -31,48 +33,91 @@ public class IntersceneLma {
this.opticalFlow = opticalFlow;
}
public double [] getSceneXYZ() {
double [] full_vector = getFullVector(parameters_vector);
public double [] getSceneXYZ(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getSceneATR(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getReferenceXYZ(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getReferenceATR(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getSceneERSXYZ(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getSceneERSATR(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getReferenceERSXYZ(boolean initial) {
double [] full_vector = initial? backup_parameters_full: 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);
public double [] getReferenceERSATR(boolean initial) {
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] {
full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]};
}
public String [] printOldNew(boolean allvectors) {
return printOldNew(allvectors, 8, 5);
}
public String [] printOldNew(boolean allvectors, int w, int d) {
ArrayList<String> lines = new ArrayList<String>();
for (int n = ErsCorrection.DP_DVAZ; n < ErsCorrection.DP_NUM_PARS; n+=3) {
boolean adj = false;
for (int i = 0; i <3; i++) adj |= par_mask[n+i];
if (allvectors || adj) {
String line = printNameV3(n, false, w,d)+" (was "+printNameV3(n, true, w,d)+")";
lines.add(line);
}
}
return lines.toArray(new String[lines.size()]);
}
public String printNameV3(int indx, boolean initial, int w, int d) {
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
double [] vector = new double[3];
for (int i = 0; i <3; i++) {
vector[i] = full_vector[indx + i];
}
String name = ErsCorrection.DP_VECTORS_NAMES[indx];
return printNameV3(name, vector, w, d);
}
public static String printNameV3(String name, double[] vector) {
return printNameV3(name, vector, 8, 5);
}
public static String printNameV3(String name, double[] vector, int w, int d) {
return String.format("%14s: %s", name, printV3(vector, w, d));
}
public static String printV3(double[] vector) {
return printV3(vector, 8, 5);
}
public static String printV3(double[] vector, int w, int d) {
String fmt = String.format("[%%%d.%df, %%%d.%df, %%%d.%df]", w,d,w,d,w,d);
return String.format(fmt, vector[0], vector[1], vector[2]);
}
public void prepareLMA(
......@@ -85,6 +130,7 @@ public class IntersceneLma {
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
boolean first_run,
final int debug_level)
{
scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
......@@ -108,7 +154,10 @@ public class IntersceneLma {
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]};
parameters_full = full_parameters_vector.clone();
if (first_run || (backup_parameters_full == null)) {
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];
......@@ -116,7 +165,7 @@ public class IntersceneLma {
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();
// parameters_initial = parameters_vector.clone();
setSamplesWeights(vector_XYS); // not regularization yet !
......@@ -158,13 +207,14 @@ public class IntersceneLma {
good_or_bad_rms = this.last_rms.clone();
}
public boolean runLma(
public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
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
boolean last_run,
int debug_level)
{
boolean [] rslt = {false,false};
......@@ -176,7 +226,7 @@ public class IntersceneLma {
rms_diff,
debug_level);
if (rslt == null) {
return false; // need to check
return -1; // 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]+
......@@ -212,7 +262,16 @@ public class IntersceneLma {
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];
if (debug_level > 0) {
if ((debug_level > 1) || (iter == 1) || last_run) {
String [] lines = printOldNew(false); // boolean allvectors)
for (String line : lines) {
System.out.println(line);
}
}
}
return rslt[0]? iter : -1;
}
private boolean [] lmaStep(
......@@ -445,7 +504,7 @@ public class IntersceneLma {
private double [] getFullVector(double [] vector) {
double [] full_vector = backup_parameters_full.clone();
double [] full_vector = parameters_full.clone();
for (int i = 0; i < par_indices.length; i++) {
full_vector[par_indices[i]] = vector[i];
}
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneLmaParameters.java
View file @ 3f43f2ac
......@@ -13,15 +13,59 @@ public class IntersceneLmaParameters {
public double ilma_lambda_max = 100;
public double ilma_rms_diff = 0.001;
public int ilma_num_iter = 20;
public int ilma_num_corr = 10; // maximal number of full correlatiobn+LMA cycles
public int ilma_debug_level = 1;
public IntersceneLmaParameters() {
ilma_lma_select[ErsCorrection.DP_DVAZ]= true;
ilma_lma_select[ErsCorrection.DP_DVTL]= true;
ilma_lma_select[ErsCorrection.DP_DVRL]= true;
ilma_lma_select[ErsCorrection.DP_DVX]= true;
ilma_lma_select[ErsCorrection.DP_DVY]= true;
ilma_lma_select[ErsCorrection.DP_DVZ]= true;
ilma_lma_select[ErsCorrection.DP_DAZ]= false;
ilma_lma_select[ErsCorrection.DP_DTL]= false;
ilma_lma_select[ErsCorrection.DP_DRL]= false;
ilma_lma_select[ErsCorrection.DP_DX]= false;
ilma_lma_select[ErsCorrection.DP_DY]= false;
ilma_lma_select[ErsCorrection.DP_DZ]= false;
ilma_lma_select[ErsCorrection.DP_DSVAZ]= true;
ilma_lma_select[ErsCorrection.DP_DSVTL]= true;
ilma_lma_select[ErsCorrection.DP_DSVRL]= true;
ilma_lma_select[ErsCorrection.DP_DSVX]= true;
ilma_lma_select[ErsCorrection.DP_DSVY]= true;
ilma_lma_select[ErsCorrection.DP_DSVZ]= true;
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;
ilma_regularization_weights[ErsCorrection.DP_DVAZ]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DVTL]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DVRL]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DVX]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DVY]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DVZ]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DAZ]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DTL]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DRL]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DX]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DY]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DZ]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSVAZ]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSVTL]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSVRL]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSVX]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSVY]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSVZ]= 0.001;
ilma_regularization_weights[ErsCorrection.DP_DSAZ]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSTL]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSRL]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSX]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSY]= 0.0;
ilma_regularization_weights[ErsCorrection.DP_DSZ]= 0.0;
}
public void dialogQuestions(GenericJTabbedDialog gd) {
......@@ -49,6 +93,10 @@ public class IntersceneLmaParameters {
"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("Maximal number of correlation +LMA iterations",this.ilma_num_corr, 0,3,"",
"Outer cycle (recalculate correlations + lma). Otherwise exits if LMA exits at first iteration.");
gd.addNumericField("Debug level", this.ilma_debug_level, 0,3,"",
"Debug level of interscene LMA operation.");
}
......@@ -65,6 +113,7 @@ public class IntersceneLmaParameters {
this.ilma_lambda_max = gd.getNextNumber();
this.ilma_rms_diff = gd.getNextNumber();
this.ilma_num_iter = (int) gd.getNextNumber();
this.ilma_num_corr = (int) gd.getNextNumber();
this.ilma_debug_level = (int) gd.getNextNumber();
}
public void setProperties(String prefix,Properties properties){
......@@ -78,6 +127,7 @@ public class IntersceneLmaParameters {
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_num_corr", this.ilma_num_corr+"");
properties.setProperty(prefix+"ilma_debug_level", this.ilma_debug_level+"");
}
public void getProperties(String prefix,Properties properties){
......@@ -94,6 +144,7 @@ public class IntersceneLmaParameters {
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_num_corr")!=null) this.ilma_num_corr=Integer.parseInt(properties.getProperty(prefix+"ilma_num_corr"));
if (properties.getProperty(prefix+"ilma_debug_level")!=null) this.ilma_debug_level=Integer.parseInt(properties.getProperty(prefix+"ilma_debug_level"));
}
......@@ -108,6 +159,7 @@ public class IntersceneLmaParameters {
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_num_corr = this.ilma_num_corr;
ilp.ilma_debug_level = this.ilma_debug_level;
return ilp;
}
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 3f43f2ac
......@@ -296,10 +296,13 @@ public class OpticalFlow {
return flowXYS;
}
private void removeOutliers(
private int removeOutliers(
double nsigma,
double [][] flowXYS)
{
if (nsigma < 0.0) {
return 0;
}
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];
......@@ -330,14 +333,17 @@ public class OpticalFlow {
n++;
}
}
System.out.println("Removed "+n+" outliers");
return n;
/*
double err = 4.0;
for (int i = 0; i < flowXYS.length; i++) if (flowXYS[i] != null){
flowXYS[i][0] += err;
flowXYS[i][1] += err;
}
*/
}
return -1;
}
......@@ -589,14 +595,11 @@ public class OpticalFlow {
// undefine tiles in flowXY that are never used
if (ntry == 0) {
for (int i = 0; i <flowXY.length; i++) {
// if (flowXY_run[i] == null) {
if ((scene_tiles[i] == null) || (reference_tiles[i] == null)) {
flowXY[i] = null;
}
}
}
//
double [][] corr2dscene_ref = correlate2DSceneToReference(// to match to reference
// imgdtt_params, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
scene_QuadClt, // final QuadCLT scene_QuadClt,
......@@ -931,7 +934,7 @@ public class OpticalFlow {
if (iYMmax >= transform_size) {
iYMmax = transform_size -1;
}
if (iYMmax >= transform_size) {
if (iXMmax >= transform_size) {
iXMmax = transform_size -1;
}
double dMax = corr2d_tile[iYMmax * corr_size + iXMmax]; // negative
......@@ -1324,7 +1327,7 @@ public class OpticalFlow {
double wdiag = 0.25 *diagonal_weight / (diagonal_weight + 1.0);
double wortho = 0.25 / (diagonal_weight + 1.0);
final double [] neibw = {wortho, wdiag, wortho, wdiag, wortho, wdiag, wortho, wdiag};
final int dbg_mtile = (debug_level > 0)? 54 : -1; // 453; // 500;
final int dbg_mtile = (debug_level > 0)? 54: -1; // 54 : -1; // 453; // 500;//250;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
......@@ -1369,10 +1372,11 @@ 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)
// xyd[0], xyd[1] are here offset by transform_size/2 (to the center of the tile)
if (xyd != null) {
tXtYD[iTile] = new double [] {
((xyd[0] + flowXY[iMTile][0])/transform_size),
((xyd[1] + flowXY[iMTile][1])/transform_size),
((xyd[0] + flowXY[iMTile][0])/transform_size) - 0.5, // moving 0.5 tiles back
((xyd[1] + flowXY[iMTile][1])/transform_size) - 0.5, // moving 0.5 tiles back
xyd[2]};
} else {
tXtYD[iTile] = null;
......@@ -1713,7 +1717,7 @@ public class OpticalFlow {
scene_QuadClt, // final QuadCLT qthis,
margin); // final int margin); // extra margins over 16x16 tiles to accommodate distorted destination tiles
*/
showComareMacroTiles(
showCompareMacroTiles(
title, // String title,
new double [][][][] {reference_tiles, scene_tiles}, // double [][][][] source_tiles_sets,
scene_QuadClt, // final QuadCLT qthis,
......@@ -1974,6 +1978,71 @@ public class OpticalFlow {
}
/**
* Calculate and display comparison stack of reference and scene images
* @param suffix Add this text to the end of image name
* blur_reference Process and blur the reference image same as the scene one
* @param camera_xyz0 scene camera offset in world coordinates
* @param camera_atr0 scene camera orientation in world coordinates
* @param reference_QuadCLT reference scene instance
* @param scene_QuadCLT scene instance
* @param iscale upsample for interpolation
*/
public void compareRefSceneTiles(
String suffix,
boolean blur_reference,
double [] camera_xyz0,
double [] camera_atr0,
QuadCLT reference_QuadCLT,
QuadCLT scene_QuadCLT,
int iscale) // 8
{
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;
if (blur_reference) {
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);
} else {
dsrbg_ref= reference_QuadCLT.getDSRBG();
}
double [][][] pair = {dsrbg_ref, dsrbg};
TileProcessor tp = reference_QuadCLT.getTileProcessor();
int tilesX = tp.getTilesX();
int tilesY = tp.getTilesY();
String [] dsrbg_titles = {"d", "s", "r", "b", "g"};
// combine this scene with warped previous one
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 = reference_QuadCLT.getImageName()+"-"+scene_QuadCLT.image_name+suffix;
(new ShowDoubleFloatArrays()).showArrays(
dbg_rslt,
tilesX,
tilesY,
true,
title,
rtitles);
}
/**
* Show macrotiles in comparison, typically reference to scene ones
* @param title Image stack title.
......@@ -1982,7 +2051,7 @@ public class OpticalFlow {
* @param qthis Scene instance to extract dimensions
* @param margin Extra margin around the tiles (not used currently, always 0)
*/
public void showComareMacroTiles(
public void showCompareMacroTiles(
String title,
double [][][][] source_tiles_sets,
final QuadCLT qthis,
......@@ -2137,21 +2206,6 @@ 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
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
ZERO3, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
ZERO3, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
true, // boolean distortedCamera, // camera view is distorted (false - rectilinear)
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
......@@ -2164,7 +2218,7 @@ public class OpticalFlow {
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)
0.5); // LINE_ERR); // double line_err) // threshold error in scan lines (1.0)
if (pXpYD != null) {
int px = (int) Math.round(pXpYD[0]/transform_size);
......@@ -2242,6 +2296,427 @@ public class OpticalFlow {
return dsrbg_out;
}
public void adjustPairsDualPass(
CLTParameters clt_parameters,
double k_prev,
QuadCLT [] scenes, // ordered by increasing timestamps
int debug_level
)
{
double scale_two_omegas = 2.0; // ers angular velocities contain double omegas
double [][][] scenes_xyzatr = new double [scenes.length][][]; // previous scene relative to the next one
double [][][] ers_xyzatr = new double [scenes.length][][]; // previous scene relative to the next one
// pass I - adjust pairs using angular and linear velocities from intrascene ERS.
for (int i = 1; i < scenes.length; i++) {
QuadCLT reference_QuadClt = scenes[i];
QuadCLT scene_QuadClt = scenes[i - 1];
double [][] pose = getPoseFromErs(
k_prev,
reference_QuadClt,
scene_QuadClt,
debug_level);
reference_QuadClt.getErsCorrection().setupERSfromExtrinsics();
scene_QuadClt.getErsCorrection().setupERSfromExtrinsics();
scenes_xyzatr[i] = adjustPairsLMA(
clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT,
scene_QuadClt, // QuadCLT scene_QuadCLT,
pose[0], // xyz
pose[1], // atr
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
debug_level); // int debug_level)
if (debug_level > -1) {
System.out.println("Pass 1 scene "+i+" (of "+ scenes.length+") "+
reference_QuadClt.getImageName() + "/" + scene_QuadClt.getImageName()+" Done.");
}
}
for (int i = 0; i < scenes.length; i++) {
int i_prev = i - ((i > 0) ? 1 : 0);
int i_next = i + ((i < (scenes.length - 1)) ? 1 : 0);
double dt = scenes[i_next].getTimeStamp() - scenes[i_prev].getTimeStamp();
ers_xyzatr[i] = new double[2][3];
if (i>0) {
for (int j = 0; j < 3; j++) {
ers_xyzatr[i][0][j] += scenes_xyzatr[i][0][j];
ers_xyzatr[i][1][j] += scenes_xyzatr[i][1][j];
}
}
if (i < (scenes.length - 1)) {
for (int j = 0; j < 3; j++) {
ers_xyzatr[i][0][j] += scenes_xyzatr[i + 1][0][j];
ers_xyzatr[i][1][j] += scenes_xyzatr[i + 1][1][j];
}
}
for (int j = 0; j < 3; j++) {
ers_xyzatr[i][0][j] *= 1.0 / dt;
ers_xyzatr[i][1][j] *= scale_two_omegas / dt;
}
ers_xyzatr[i][1][0] = -ers_xyzatr[i][1][0]; /// TESTING!
ers_xyzatr[i][1][2] = -ers_xyzatr[i][1][2]; /// TESTING!
}
//{camera_xyz0, camera_atr0}
// pass II - set scene velocities from offsets to 1 before and one after, freeze ERS parameters, and
// adjust other ones.
boolean[] param_select2 = clt_parameters.ilp.ilma_lma_select.clone(); // final boolean[] param_select,
double [] param_regweights2 = clt_parameters.ilp.ilma_regularization_weights; // final double [] param_regweights,
// freeze reference ERS, free scene ERS
for (int j = 0; j <3; j++) {
param_select2[ErsCorrection.DP_DVX + j] = false;
param_select2[ErsCorrection.DP_DVAZ + j] = false;
param_regweights2[ErsCorrection.DP_DSVX + j] = 0.0;
param_regweights2[ErsCorrection.DP_DSVAZ + j] = 0.0;
}
double [][][] scenes_xyzatr1 = new double [scenes.length][][]; // previous scene relative to the next one
for (int i = 1; i < scenes.length; i++) {
QuadCLT reference_QuadClt = scenes[i];
QuadCLT scene_QuadClt = scenes[i - 1];
double [][] pose = scenes_xyzatr[i];
ErsCorrection ers_reference = reference_QuadClt.getErsCorrection();
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
ers_reference.ers_wxyz_center_dt = ers_xyzatr[i][0].clone();
ers_reference.ers_watr_center_dt = ers_xyzatr[i][1].clone();
int i_prev = i - ((i > 0) ? 1 : 0);
ers_reference.setupERS(); // just in case - setUP using instance paRAMETERS
ers_scene.ers_wxyz_center_dt = ers_xyzatr[i_prev][0].clone();
ers_scene.ers_watr_center_dt = ers_xyzatr[i_prev][1].clone();
ers_scene.setupERS(); // just in case - setUP using instance paRAMETERS
scenes_xyzatr1[i] = adjustPairsLMA(
clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT,
scene_QuadClt, // QuadCLT scene_QuadCLT,
pose[0], // xyz
pose[1], // atr
param_select2, // final boolean[] param_select,
param_regweights2, // final double [] param_regweights,
debug_level); // int debug_level)
ers_reference.addScene(scene_QuadClt.getImageName(),
scenes_xyzatr1[i][0],
scenes_xyzatr1[i][1],
ers_scene.getErsXYZ_dt(),
ers_scene.getErsATR_dt()
);
if (debug_level > -1) {
System.out.println("Pass 2 scene "+i+" (of "+ scenes.length+") "+
reference_QuadClt.getImageName() + "/" + scene_QuadClt.getImageName()+" Done.");
}
}
for (int i = 1; i < scenes.length; i++) {
QuadCLT reference_QuadClt = scenes[i];
/*
QuadCLT scene_QuadClt = scenes[i - 1];
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
reference_QuadClt.getErsCorrection().addScene(scene_QuadClt.getImageName(),
scenes_xyzatr1[i][0],
scenes_xyzatr1[i][1],
ers_scene.getErsXYZ_dt(),
ers_scene.getErsATR_dt()
);
*/
reference_QuadClt.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debug_level+1);
}
}
double [][] getPoseFromErs(
double k_prev,
QuadCLT reference_QuadCLT,
QuadCLT scene_QuadCLT,
int debug_level)
{
ErsCorrection ersCorrection = reference_QuadCLT.getErsCorrection();
String this_image_name = reference_QuadCLT.getImageName();
if (debug_level > 0) {
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;
}
ErsCorrection ersCorrectionPrev = (ErsCorrection) (scene_QuadCLT.geometryCorrection);
dt = reference_QuadCLT.getTimeStamp() - scene_QuadCLT.getTimeStamp();
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);
}
double [] wxyz_center_dt_prev = ersCorrectionPrev.ers_wxyz_center_dt;
double [] watr_center_dt_prev = ersCorrectionPrev.ers_watr_center_dt; // is twice omega!
double [] wxyz_delta = new double[3];
double [] watr_delta = new double[3];
for (int i = 0; i <3; i++) {
wxyz_delta[i] = dt * (k_prev * wxyz_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_wxyz_center_dt[i]);
watr_delta[i] = 0.5 * dt * (k_prev * watr_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_watr_center_dt[i]);
}
watr_delta[0] = -watr_delta[0]; /// TESTING!
watr_delta[2] = -watr_delta[2]; /// TESTING!
return new double [][] {wxyz_delta, watr_delta};
}
public double[][] adjustPairsLMA(
CLTParameters clt_parameters,
// double k_prev,
QuadCLT reference_QuadCLT,
QuadCLT scene_QuadCLT,
double [] camera_xyz0,
double [] camera_atr0,
boolean[] param_select,
double [] param_regweights,
int debug_level)
{
TileProcessor tp = reference_QuadCLT.getTileProcessor();
final int iscale = 8;
boolean blur_reference = false;
/*
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();
if (debug_level > 0) {
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;
}
ErsCorrection ersCorrectionPrev = (ErsCorrection) (scene_QuadCLT.geometryCorrection);
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);
}
double [] wxyz_center_dt_prev = ersCorrectionPrev.ers_wxyz_center_dt;
double [] watr_center_dt_prev = ersCorrectionPrev.ers_watr_center_dt; // is twice omega!
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]);
wxyz_delta[i] = dt * (k_prev * wxyz_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_wxyz_center_dt[i]);
watr_delta[i] = 0.5 * dt * (k_prev * watr_center_dt_prev[i] + (1.0-k_prev) * ersCorrection.ers_watr_center_dt[i]);
}
watr_delta[0] = -watr_delta[0]; /// TESTING!
watr_delta[2] = -watr_delta[2]; /// TESTING!
camera_xyz0 = wxyz_delta;
camera_atr0 = watr_delta;
}
*/
int tilesX = tp.getTilesX();
int tilesY = tp.getTilesY();
if (debug_level > 0) {
compareRefSceneTiles(
"before_LMA", // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
IntersceneLma intersceneLma = new IntersceneLma(
this); // OpticalFlow opticalFlow
int nlma = 0;
for (nlma = 0; nlma < clt_parameters.ilp.ilma_num_corr; nlma++) {
boolean last_run = nlma == ( clt_parameters.ilp.ilma_num_corr - 1);
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()+"-iteration_"+nlma;;
showVectorXYConfidence(
dbg_title, // String title,
vector_XYS, // double [][] flowXYS,
macroTilesX); // int width)
}
int n = removeOutliers(
clt_parameters.ofp.nsigma, // double nsigma, 1.5 - 2.0
vector_XYS); // double [][] flowXYS)
if (debug_level > 1) {
System.out.println("Removed "+n+" outliers");
}
int n2 = removeOutliers(
clt_parameters.ofp.nsigma2, // double nsigma, 1.5 - 2.0
vector_XYS); // double [][] flowXYS)
if (debug_level > 1) {
System.out.println("Removed "+n2+" outliers in a second pass, total removed:"+(n+n2));
}
if (debug_level > 0) {
if ((debug_level > 1) || (nlma == 0)) {
String dbg_title = "OpticalFlowFiltered-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName()+"-iteration_"+nlma;
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,
param_select, // final boolean[] param_select,
param_regweights, // 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
(nlma == 0), // boolean first_run,
debug_level); // final int debug_level)
int lmaResult = 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
last_run, // boolean last_run,
clt_parameters.ilp.ilma_debug_level); // int debug_level)
if (lmaResult < 0) {
System.out.println("LMA failed");
break;
}
camera_xyz0 = intersceneLma.getSceneXYZ(false); // true for initial values
camera_atr0 = intersceneLma.getSceneATR(false); // true for initial values
if (debug_level > 1) {
compareRefSceneTiles(
"iteration_"+nlma, // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
if (lmaResult <= 1) {
break;
}
}
if (debug_level == 1) {
compareRefSceneTiles(
"after_lma", // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
/*
double [] wxyz_center_dt_prev1 = ersCorrectionPrev.ers_wxyz_center_dt;
double [] watr_center_dt_prev1 = ersCorrectionPrev.ers_watr_center_dt; // is twice omega!
double [] wxyz_delta1 = new double[3];
double [] watr_delta1 = new double[3];
for (int i = 0; i <3; i++) {
wxyz_delta1[i] = dt * (k_prev * wxyz_center_dt_prev1[i] + (1.0-k_prev) * ersCorrection.ers_wxyz_center_dt[i]);
watr_delta1[i] = 0.5 * dt * (k_prev * watr_center_dt_prev1[i] + (1.0-k_prev) * ersCorrection.ers_watr_center_dt[i]);
}
watr_delta1[0] = -watr_delta1[0]; /// TESTING!
watr_delta1[2] = -watr_delta1[2]; /// TESTING!
if (debug_level > 0) {
System.out.println(IntersceneLma.printNameV3("ATR from ERS", watr_delta1));
System.out.println(IntersceneLma.printNameV3("XYZ from ERS", wxyz_delta1));
System.out.println("Number of full corr+LMA runs = "+(nlma+1));
}
*/
/*
reference_QuadCLT.getErsCorrection().addScene(scene_QuadCLT.getImageName(), camera_xyz0,camera_atr0);
reference_QuadCLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debug_level);
*/
return new double [][] {camera_xyz0, camera_atr0};
// return null; //pair;
}
public double[][][] test_LMA(
CLTParameters clt_parameters,
double k_prev,
......@@ -2252,6 +2727,7 @@ public class OpticalFlow {
{
TileProcessor tp = reference_QuadCLT.getTileProcessor();
final int iscale = 8;
boolean blur_reference = false;
double ts = reference_QuadCLT.getTimeStamp();
double ts_prev = ts;
double [] camera_xyz0 = ZERO3.clone();
......@@ -2301,50 +2777,20 @@ public class OpticalFlow {
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);
if (debug_level > 0) {
compareRefSceneTiles(
"before_LMA", // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
// Add limitations on disparity ? To be used with multi-tile consolidation
// Check with walking, not only rotating
IntersceneLma intersceneLma = new IntersceneLma(
this); // OpticalFlow opticalFlow
for (int nlma = 0; nlma < clt_parameters.ilp.ilma_num_corr; nlma++) {
boolean last_run = nlma == ( clt_parameters.ilp.ilma_num_corr - 1);
int transform_size = tp.getTileSize();
int macroTilesX = tilesX/transform_size;
int macroTilesY = tilesY/transform_size;
......@@ -2385,27 +2831,35 @@ public class OpticalFlow {
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();
if (debug_level > 2) {
String dbg_title = "OpticalFlow-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName()+"-iteration_"+nlma;;
showVectorXYConfidence(
dbg_title, // String title,
vector_XYS, // double [][] flowXYS,
macroTilesX); // int width)
}
double nsigma = 1.7;
removeOutliers(
nsigma, // double nsigma,
int n = removeOutliers(
clt_parameters.ofp.nsigma, // double nsigma, 1.5 - 2.0
vector_XYS); // double [][] flowXYS)
if (debug_level > -2) {
String dbg_title = "OpticalFlowFiltered-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
if (debug_level > -1) {
System.out.println("Removed "+n+" outliers");
}
int n2 = removeOutliers(
clt_parameters.ofp.nsigma2, // double nsigma, 1.5 - 2.0
vector_XYS); // double [][] flowXYS)
if (debug_level > -1) {
System.out.println("Removed "+n2+" outliers in a second pass, total removed:"+(n+n2));
}
if (debug_level > 1) {
String dbg_title = "OpticalFlowFiltered-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName()+"-iteration_"+nlma;
showVectorXYConfidence(
dbg_title, // String title,
vector_XYS, // double [][] flowXYS,
macroTilesX); // int width)
}
IntersceneLma intersceneLma = new IntersceneLma(
this); // OpticalFlow opticalFlow
// 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)
......@@ -2417,27 +2871,55 @@ public class OpticalFlow {
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
(nlma == 0), // boolean first_run,
debug_level); // final int debug_level)
boolean lmaOK = intersceneLma.runLma(
int lmaResult = 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
last_run, // boolean last_run,
clt_parameters.ilp.ilma_debug_level); // int debug_level)
if (!lmaOK) {
if (lmaResult < 0) {
System.out.println("LMA failed");
break;
}
camera_xyz0 = intersceneLma.getSceneXYZ();
camera_atr0 = intersceneLma.getSceneATR();
camera_xyz0 = intersceneLma.getSceneXYZ(false); // true for initial values
camera_atr0 = intersceneLma.getSceneATR(false); // true for initial values
if (debug_level > 1) {
compareRefSceneTiles(
"iteration_"+nlma, // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
if (lmaResult <= 1) {
break;
}
return null; //pair;
}
if (debug_level == 1) {
compareRefSceneTiles(
"after_lma", // String suffix,
blur_reference, // boolean blur_reference,
camera_xyz0, // double [] camera_xyz0,
camera_atr0, // double [] camera_atr0,
reference_QuadCLT, // QuadCLT reference_QuadCLT,
scene_QuadCLT, // QuadCLT scene_QuadCLT,
iscale); // int iscale) // 8
}
reference_QuadCLT.getErsCorrection().addScene(scene_QuadCLT.getImageName(), camera_xyz0,camera_atr0);
reference_QuadCLT.saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debug_level);
return null;
}
// ErsCorrection ersCorrection = reference_QuadCLT.getErsCorrection();
/**
* A top-level method for testing optical flow generating, currently includes temporary testing functionality
......@@ -2743,7 +3225,7 @@ public class OpticalFlow {
//reference_tiles
double [][][][] scene_to_ref = {reference_tiles, scene_tiles};
if (debug_level > 0) {
showComareMacroTiles(
showCompareMacroTiles(
"tiles_scene-"+scene_QuadCLT.getImageName()+"-ref"+reference_QuadCLT.getImageName(),// String title,
scene_to_ref, // double [][][][] source_tiles_sets,
reference_QuadCLT, // final QuadCLT qthis,
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlowParameters.java
View file @ 3f43f2ac
......@@ -39,6 +39,8 @@ public class OpticalFlowParameters {
public double tolerance_relative_inter = 0.2; // relative disparity half-range in each tile
public double occupancy_inter = 0.25; // fraction of remaining tiles in the center 8x8 area (<1.0)
public double nsigma = 1.5; // Remove outliers by more that this scaled sigma
public double nsigma2 = 2.0; // Second time outlier filter (<0 - disable)
public double [] chn_weights = {1.0,1.0,1.0,1.0}; // strength, r,b,g
// double [] chn_weights = {1.0,0.0,0.0,0.0}; // strength, r,b,g
// double [] chn_weights = {0.0,1.0,1.0,1.0}; // strength, r,b,g
......@@ -63,6 +65,7 @@ public class OpticalFlowParameters {
public boolean combine_empty_only = true; // false;
public boolean late_normalize_iterate = true;
public int test_corr_rad_max = 3;
// for recalculateFlowXY()
......@@ -97,6 +100,10 @@ public class OpticalFlowParameters {
gd.addMessage("Interscene 2D correlation");
gd.addNumericField("Remove outliers (relative to sigma)", this.nsigma, 3,6,"",
"Remove optical flow macrotiles that differ from weighted average by more that this factor scaled standard deviation");
gd.addNumericField("Remove outliers (relative to sigma), second pass (-1 disable)", this.nsigma2, 3,6,"",
"Second pass of outlier removal (with new sigma). Set to -1 to disable second pass");
gd.addNumericField("Correlation weight of the intrascene strength channel", this.chn_weights[0], 3,6,"",
"Weight of the intrascene correlation strength in interscene macrotile correlation (will be normalized)");
gd.addNumericField("Correlation weight of the intrascene red channel", this.chn_weights[1], 3,6,"",
......@@ -166,6 +173,8 @@ public class OpticalFlowParameters {
this.tolerance_absolute_inter = gd.getNextNumber();
this.tolerance_relative_inter = gd.getNextNumber();
this.occupancy_inter = gd.getNextNumber();
this.nsigma = gd.getNextNumber();
this.nsigma2 = gd.getNextNumber();
this.chn_weights[0] = gd.getNextNumber();
this.chn_weights[1] = gd.getNextNumber();
this.chn_weights[2] = gd.getNextNumber();
......@@ -207,6 +216,8 @@ public class OpticalFlowParameters {
properties.setProperty(prefix+"tolerance_absolute_inter", this.tolerance_absolute_inter+"");
properties.setProperty(prefix+"tolerance_relative_inter", this.tolerance_relative_inter+"");
properties.setProperty(prefix+"occupancy_inter", this.occupancy_inter+"");
properties.setProperty(prefix+"nsigma", this.nsigma+"");
properties.setProperty(prefix+"nsigma2", this.nsigma2+"");
for (int i = 0; i < chn_weights.length; i++) {
properties.setProperty(prefix+"chn_weights_"+i, this.chn_weights[i]+"");
}
......@@ -246,6 +257,8 @@ public class OpticalFlowParameters {
if (properties.getProperty(prefix+"tolerance_absolute_inter")!=null) this.tolerance_absolute_inter=Double.parseDouble(properties.getProperty(prefix+"tolerance_absolute_inter"));
if (properties.getProperty(prefix+"tolerance_relative_inter")!=null) this.tolerance_relative_inter=Double.parseDouble(properties.getProperty(prefix+"tolerance_relative_inter"));
if (properties.getProperty(prefix+"occupancy_inter")!=null) this.occupancy_inter=Double.parseDouble(properties.getProperty(prefix+"occupancy_inter"));
if (properties.getProperty(prefix+"nsigma")!=null) this.nsigma=Double.parseDouble(properties.getProperty(prefix+"nsigma"));
if (properties.getProperty(prefix+"nsigma2")!=null) this.nsigma2=Double.parseDouble(properties.getProperty(prefix+"nsigma2"));
for (int i = 0; i < chn_weights.length; i++) {
String s_chn_weight = "chn_weights_"+i;
if (properties.getProperty(prefix+s_chn_weight)!=null) this.chn_weights[i]=Double.parseDouble(properties.getProperty(prefix+s_chn_weight));
......@@ -287,6 +300,8 @@ public class OpticalFlowParameters {
ofp.tolerance_absolute_inter = this.tolerance_absolute_inter;
ofp.tolerance_relative_inter = this.tolerance_relative_inter;
ofp.occupancy_inter = this.occupancy_inter;
ofp.nsigma = this.nsigma;
ofp.nsigma2 = this.nsigma2;
ofp.chn_weights = this.chn_weights.clone();
ofp.corr_sigma = this.corr_sigma;
ofp.fat_zero = this.fat_zero;
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ 3f43f2ac
......@@ -113,7 +113,7 @@ public class QuadCLT extends QuadCLTCPU {
restoreDSI("-DSI_MAIN"); // "-DSI_COMBO", "-DSI_MAIN" (DSI_COMBO_SUFFIX, DSI_MAIN_SUFFIX)
restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or null to use x3d directory
// null, // Properties properties, // if null - will only save extrinsics)
false, // boolean all_properties,// null, // Properties properties, // if null - will only save extrinsics)
debugLevel);
// showDSIMain();
return this; // can only be QuadCLT instance
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 3f43f2ac
......@@ -340,11 +340,12 @@ public class QuadCLTCPU {
public Properties restoreInterProperties( // restore properties for interscene processing (extrinsics, ers, ...)
String path, // full name with extension or null to use x3d directory
// Properties properties, // if null - will only save extrinsics)
boolean all_properties,
int debugLevel)
{
if (path == null) {
path = image_name + ((properties == null) ? "-INTERFRAME":"")+".corr-xml";
// path = image_name + ((properties == null) ? "-INTERFRAME":"")+".corr-xml";
path = image_name + (all_properties? "": "-INTERFRAME")+".corr-xml";
}
if (!path.contains(Prefs.getFileSeparator())) {
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 3f43f2ac
......@@ -8281,6 +8281,117 @@ if (debugLevel > -100) return true; // temporarily !
}
public void interPairsLMA(
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,
boolean reset_from_extrinsics,
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());
ErsCorrection ers = quadCLTs[i].getErsCorrection();
if (reset_from_extrinsics) {
System.out.println("Reset ERS parameters from intraframe extrinsics");
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 =
double [][] pose = opticalFlow.getPoseFromErs(
clt_parameters.ofp.k_prev, // k_prev,
quadCLTs[i],
qPrev,
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
// how was it working before? qPrev.getErsCorrection() shoud remain what was set in the previous adjustment
quadCLTs[i].getErsCorrection().setupERSfromExtrinsics();
qPrev.getErsCorrection().setupERSfromExtrinsics();
opticalFlow.adjustPairsLMA(
clt_parameters, // CLTParameters clt_parameters,
// clt_parameters.ofp.k_prev, // k_prev,
quadCLTs[i],
qPrev,
pose[0], // xyz
pose[1], // atr
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
// clt_parameters.ofp.ers_to_pose_scale, // corr_scale,
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
/*
scenes_xyzatr[i] = adjustPairsLMA(
clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT,
scene_QuadClt, // QuadCLT scene_QuadCLT,
pose[0], // xyz
pose[1], // atr
clt_parameters.ilp.ilma_lma_select, // final boolean[] param_select,
clt_parameters.ilp.ilma_regularization_weights, // final double [] param_regweights,
debug_level); // int debug_level)
*
*
* reversed
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 TestInterLMA(
QuadCLT quadCLT_main, // tiles should be set
CLTParameters clt_parameters,
......@@ -8291,6 +8402,7 @@ if (debugLevel > -100) return true; // temporarily !
EyesisCorrectionParameters.RGBParameters rgbParameters,
EyesisCorrectionParameters.EquirectangularParameters equirectangularParameters,
Properties properties,
boolean reset_from_extrinsics,
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel) throws Exception
......@@ -8320,7 +8432,10 @@ if (debugLevel > -100) return true; // temporarily !
debugLevel);
// temporarily fix wrong sign:
ErsCorrection ers = (ErsCorrection) (quadCLTs[i].getGeometryCorrection());
if (reset_from_extrinsics) {
System.out.println("Reset ERS parameters from intraframe extrinsics");
ers.setupERSfromExtrinsics();
}
quadCLTs[i].setDSRBG(
clt_parameters, // CLTParameters clt_parameters,
threadsMax, // int threadsMax, // maximal number of threads to launch
......@@ -8334,10 +8449,17 @@ if (debugLevel > -100) return true; // temporarily !
threadsMax, // int threadsMax, // maximal number of threads to launch
updateStatus); // boolean updateStatus);
opticalFlow.adjustPairsDualPass(
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.ofp.k_prev, // k_prev,
quadCLTs, // QuadCLT [] scenes, // ordered by increasing timestamps
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
/*
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,
......@@ -8346,6 +8468,7 @@ if (debugLevel > -100) return true; // temporarily !
clt_parameters.ofp.ers_to_pose_scale, // corr_scale,
clt_parameters.ofp.debug_level_optical); // 1); // -1); // int debug_level);
*/
/* reversed
opticalFlow.test_LMA(
clt_parameters, // CLTParameters clt_parameters,
clt_parameters.ofp.k_prev, // k_prev,
......@@ -8353,8 +8476,10 @@ if (debugLevel > -100) return true; // temporarily !
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");
......
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