Commit b469f1c2 authored by Andrey Filippov's avatar Andrey Filippov

Another snapshot

parent 30330dd5
package com.elphel.imagej.gpu; package com.elphel.imagej.gpu;
import java.util.Arrays;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
public class TpTask { public class TpTask {
public int task; public int task;
// task bits 0..7 - texture neighbors (0 - N, 1 - NE, ..., 7 - NW) // task bits 0..7 - texture neighbors (0 - N, 1 - NE, ..., 7 - NW)
...@@ -232,4 +236,47 @@ public class TpTask { ...@@ -232,4 +236,47 @@ public class TpTask {
} }
return flt; return flt;
} }
public static void showTpTask(
TpTask[] tp_tasks,
int tilesX,
int tilesY,
String title) {
int numSensors = tp_tasks[0].num_sensors;
String [] titles0 = {"X-cent", "Y-cent", "Disp","Scale"};
String [] titles = new String[titles0.length+2*numSensors];
for (int i = 0; i < titles0.length; i++) {
titles[i] = titles0[i];
}
for (int i = 0; i < numSensors; i++) {
titles[titles0.length + 2*i + 0] = "X-"+i;
titles[titles0.length + 2*i + 1] = "Y-"+i;
}
double [][] data = new double [titles.length][tilesX*tilesY];
for (int i = 0; i < data.length; i++) {
Arrays.fill(data[i], Double.NaN);
}
for (int nTask = 0; nTask < tp_tasks.length; nTask++) {
TpTask task = tp_tasks[nTask];
int nTile = task.ty*tilesX+task.tx;
data [0][nTile] = task.centerXY[0];
data [1][nTile] = task.centerXY[1];
data [2][nTile] = task.target_disparity;
data [3][nTile] = task.scale;
for (int i = 0; i < numSensors; i++) {
data [titles0.length + 2*i + 0][nTile] = task.xy[i][0];
data [titles0.length + 2*i + 1][nTile] = task.xy[i][1];
}
}
ShowDoubleFloatArrays.showArrays(
data,
tilesX,
tilesY,
true,
title,
titles);
}
} }
\ No newline at end of file
...@@ -98,6 +98,13 @@ public class IntersceneLma { ...@@ -98,6 +98,13 @@ public class IntersceneLma {
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]}; full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]};
} }
public double [][] getSceneXYZATR(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]},
{full_vector[ErsCorrection.DP_DSAZ],full_vector[ErsCorrection.DP_DSTL],full_vector[ErsCorrection.DP_DSRL]}};
}
public double [] getReferenceXYZ(boolean initial) { public double [] getReferenceXYZ(boolean initial) {
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] { return new double[] {
...@@ -108,6 +115,12 @@ public class IntersceneLma { ...@@ -108,6 +115,12 @@ public class IntersceneLma {
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]}; full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]};
} }
public double [][] getReferenceXYZATR(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]},
{full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]}};
}
public double [] getSceneERSXYZ(boolean initial) { // never used public double [] getSceneERSXYZ(boolean initial) { // never used
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
...@@ -228,6 +241,144 @@ public class IntersceneLma { ...@@ -228,6 +241,144 @@ public class IntersceneLma {
return String.format(fmt, vector[0], vector[1], vector[2]); return String.format(fmt, vector[0], vector[1], vector[2]);
} }
public void prepareLMA(
final double [][] scene_xyzatr0, // camera center in world coordinates (or null to use instance)
final double [][] scene_xyzatr_pull, // if both are not null, specify target values to pull to
final double [][] ref_xyzatr, //
// reference atr, xyz are considered 0.0 - not anymore?
final QuadCLT scene_QuadClt,
final QuadCLT reference_QuadClt,
final boolean[] param_select,
final double [] param_regweights,
// now includes optional Disparity as the last element (for num_components==3)
final double [][] vector_XYSDS,// 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};
par_mask = param_select;
macrotile_centers = centers;
num_samples = num_components * centers.length;
ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
final double [] scene_xyz = (scene_xyzatr0 != null) ? scene_xyzatr0[0] : ers_scene.camera_xyz;
final double [] scene_atr = (scene_xyzatr0 != null) ? scene_xyzatr0[1] : ers_scene.camera_atr;
final double [] reference_xyz = (ref_xyzatr != null)? ref_xyzatr[0]: ers_ref.camera_xyz; // new double[3];
final double [] reference_atr = (ref_xyzatr != null)? ref_xyzatr[1]: ers_ref.camera_xyz; // 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]};
parameters_full = full_parameters_vector.clone();
if ((vector_XYSDS != null) && (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];
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]];
}
if ((scene_xyzatr_pull != null) && (scene_xyzatr_pull[0] != null) && (scene_xyzatr_pull[1] != null)) {
double [] parameters_pull_full = parameters_full.clone();
for (int i = 0; i < 3; i++) {
parameters_pull_full[ErsCorrection.DP_DSX + i] = scene_xyzatr_pull[0][i];
parameters_pull_full[ErsCorrection.DP_DSAZ + i] = scene_xyzatr_pull[1][i];
// parameters_pull =
}
parameters_pull = new double [par_indices.length];
for (int i = 0; i < par_indices.length; i++) {
parameters_pull[i] = parameters_pull_full[par_indices[i]];
}
}
if (vector_XYSDS != null) {// skip when used for the motion blur vectors, not LMA
setSamplesWeights(vector_XYSDS); // not regularized yet ! // 3d updated
} else {
weights = null; // new double[2 * centers.length];
}
last_jt = new double [parameters_vector.length][];
if (debug_level > 1) {
System.out.println("prepareLMA() 1");
}
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)
if (vector_XYSDS == null) {
return; // for MB vectors (noLMA)
}
double [][] wjtj = getWJtJlambda( // USED in lwir all NAN
0.0, // final double lambda,
last_jt); // final double [][] jt) all 0???
for (int i = 0; i < parameters_vector.length; i++) {
int indx = num_samples + i;
if (wjtj[i][i] == 0) { // why is it zero (leading to NaN)
weights[indx] = 0;
} else {
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.
if (debug_level > 1) {
System.out.println("prepareLMA() 2");
}
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)
// Why y_vector starts with initial value of fx???
y_vector = fx.clone();
for (int i = 0; i < vector_XYSDS.length; i++) {
if (vector_XYSDS[i] != null){
y_vector[num_components * i + 0] += vector_XYSDS[i][0];
y_vector[num_components * i + 1] += vector_XYSDS[i][1];
if (num_components > 2) {
y_vector[num_components * i + 2] += vector_XYSDS[i][2];
}
}
}
if (parameters_pull != null){
for (int i = 0; i < par_indices.length; i++) {
// y_vector [i + num_components * macrotile_centers.length] += parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
y_vector [i + num_components * macrotile_centers.length] = parameters_pull[i]; // - parameters_initial[i]; // scale will be combined with weights
}
}
last_rms = new double [2];
last_ymfx = getYminusFxWeighted(
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 void prepareLMA( public void prepareLMA(
final double [] scene_xyz0, // camera center in world coordinates (or null to use instance) 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) final double [] scene_atr0, // camera orientation relative to world frame (or null to use instance)
......
...@@ -2830,7 +2830,7 @@ public class OpticalFlow { ...@@ -2830,7 +2830,7 @@ public class OpticalFlow {
boolean debug_ers = false; // true; // false; // true; // true; //11.01.2022 boolean debug_ers = false; // true; // false; // true; // true; //11.01.2022
boolean ignore_ers = false; // false; boolean ignore_ers = false; // false;
TileProcessor tp = scene_QuadClt.getTileProcessor(); TileProcessor tp = scene_QuadClt.getTileProcessor();
final int tilesX = (full_woi_in==null) ? tp.getTilesX() : full_woi_in.width; // full width,includeing extra final int tilesX = (full_woi_in==null) ? tp.getTilesX() : full_woi_in.width; // full width,including extra
final int tilesY = (full_woi_in==null) ? tp.getTilesY() : full_woi_in.height; final int tilesY = (full_woi_in==null) ? tp.getTilesY() : full_woi_in.height;
final int offsetX_ref = (full_woi_in==null) ? 0 : full_woi_in.x; final int offsetX_ref = (full_woi_in==null) ? 0 : full_woi_in.x;
final int offsetY_ref = (full_woi_in==null) ? 0 : full_woi_in.y; final int offsetY_ref = (full_woi_in==null) ? 0 : full_woi_in.y;
......
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