Commit 1e0474a8 authored by Andrey Filippov's avatar Andrey Filippov

Changed disp_dist format to [quad][4], implemented and connected new LMA

parent f8a9c3d0
......@@ -733,8 +733,8 @@ public class GPUTileProcessor {
// TODO: move port coordinates out of color channel loop
double [][] centersXY_main = null;
double [][] centersXY_aux = null;
double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[2 * quad_aux][]; // used to correct 3D correlations
double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[quad_aux][]; // used to correct 3D correlations
if (geometryCorrection_main != null) {
centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(
......
......@@ -3091,7 +3091,7 @@ B = |+dy0 -dy1 -2*dy3 |
double [] pXY = mm.getPXY();
double [][] deriv = new double [2 * NUM_SENSORS][];
int dbg_index =dbg_index (pXY, dbg_decimate);
double [][] disp_dist = new double[dbg_titles_xy.length][]; // used to correct 3D correlations
double [][] disp_dist = new double[NUM_SENSORS][]; // used to correct 3D correlations
geometryCorrection.getPortsCoordinatesAndDerivatives(
geometryCorrection, // GeometryCorrection gc_main,
false, // boolean use_rig_offsets,
......
......@@ -114,7 +114,7 @@ public class Corr2dLMA {
private int ncam = 0; // number of used cameras
private int npairs=0; // number of used pairs
private int last_cam; // index of the last camera (special treatment for disparity correction)
private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
// private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)
private final Matrix [][] m_pairs = new Matrix[NUM_CAMS][NUM_CAMS];
private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
private final int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
......@@ -206,13 +206,13 @@ public class Corr2dLMA {
}
}
public void initVector( // USED in lwir
boolean adjust_width, // adjust width of the maximum
boolean adjust_scales, // adjust 2D correlation scales
boolean adjust_ellipse, // allow non-circular correlation maximums
boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities
boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities lma_adjust_wxy
double disp0, // initial value of disparity
double half_width, // A=1/(half_widh)^2
double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity
double half_width, // A=1/(half_widh)^2 lma_half_width
double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity lma_cost_wy
) {
// int [][] pindx = new int [NUM_CAMS][NUM_CAMS];
for (int f = 0; f < NUM_CAMS; f++) {
......@@ -231,6 +231,7 @@ public class Corr2dLMA {
for (Sample s:samples) { // ignore zero-weight samples
used_cameras[s.fcam]=true;
used_cameras[s.scam]=true;
/*
if (s.fcam > last_cam) {
second_last = false;
last_cam = s.fcam;
......@@ -239,6 +240,7 @@ public class Corr2dLMA {
second_last = true;
last_cam = s.scam;
}
*/
used_pairs[pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s
used_pairs_dir[s.fcam][s.scam] = true;
}
......@@ -335,8 +337,6 @@ public class Corr2dLMA {
}
public void initMatrices() { // should be called after initVector and after setMatrices
// private final Matrix [][] m_pairs = new Matrix[NUM_CAMS][NUM_CAMS];
// private final Matrix [][] m_pairs_last = new Matrix[NUM_CAMS][NUM_CAMS];
for (int f = 0; f < NUM_CAMS; f++) for (int s = 0; s < NUM_CAMS; s++) {
m_pairs[f][s] = null;
m_pairs_last[f][s] = null;
......@@ -389,7 +389,7 @@ public class Corr2dLMA {
int num_samples = samples.size();
double [] fx= new double [num_samples + 2 * NUM_CAMS];
double sqrt2 = Math.sqrt(2.0);
// double sqrt2 = Math.sqrt(2.0);
double A = av[A_INDEX];
double B = av[B_INDEX];
double C = A + av[CMA_INDEX];
......
package com.elphel.imagej.tileprocessor;
import java.util.ArrayList;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
......@@ -1751,7 +1752,103 @@ public class Correlation2d {
return rslt;
}
public Corr2dLMA corrLMA2( // USED in lwir
ImageDttParameters imgdtt_params,
double [][] corrs,
double [][] disp_dist, // per camera disparity matrix as a 1d (linescan order)
int pair_mask, // which pairs to process
boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
double sigma, // low-pass sigma to find maximum (and convex too
double xcenter, // preliminary center x in pixels for largest baseline
double vasw_pwr, // value as weight to this power,
int debug_level,
int tileX, // just for debug output
int tileY
)
{
// corrs are organized as PAIRS, some are null if not used
// for each enabled and available pair find a maximum, filter convex and create sample list
DoubleGaussianBlur gb = null;
if (sigma > 0) gb = new DoubleGaussianBlur();
int center = transform_size - 1;
int corr_size = 2 * transform_size - 1;
Corr2dLMA lma = new Corr2dLMA(transform_size);
for (int npair = 0; npair < corrs.length; npair++) if ((corrs[npair] != null) && (((pair_mask >> npair) & 1) !=0)){
double[] corr = corrs[npair].clone();
if (sigma > 0) {
gb.blurDouble(corr, corr_size, corr_size, sigma, sigma, 0.01);
}
int imx = 0;
for (int i = 1; i < corr.length; i++) if (corr[i] > corr[imx]) imx = i;
// filter convex
int ix0 = (imx % corr_size) - center; // signed, around center to match filterConvex
int iy0 = (imx / corr_size) - center; // signed, around center to match filterConvex
double [] filtWeight = filterConvex(
corr, // double [] corr_data,
imgdtt_params.cnvx_hwnd_size, // int hwin,
ix0, // int x0,
iy0, // int y0,
imgdtt_params.cnvx_add3x3, // boolean add3x3,
imgdtt_params.cnvx_weight, // double nc_cost,
(debug_level > 2)); // boolean debug);
int fcam = PAIRS[npair][0];
int scam = PAIRS[npair][1];
for (int i = 1; i < filtWeight.length; i++) if (filtWeight[i] > 0.0) {
int ix = i % corr_size; // >=0
int iy = i / corr_size; // >=0
double v = corrs[npair][i]; // not blurred
double w = filtWeight[i];
if (vasw_pwr != 0) {
w *= Math.pow(Math.abs(v), vasw_pwr);
}
lma.addSample( // x = 0, y=0 - center
fcam, // int fcam, // first camera index
scam, // int scam, // second camera index
ix, // int x, // x coordinate on the common scale (corresponding to the largest baseline), along the disparity axis
iy, // int y, // y coordinate (0 - disparity axis)
v, // double v, // correlation value at that point
w); //double w){ // sample weight
}
}
lma.initVector( // USED in lwir
imgdtt_params.lma_adjust_wm, // boolean adjust_width, // adjust width of the maximum - lma_adjust_wm
imgdtt_params.lma_adjust_ag, // boolean adjust_scales, // adjust 2D correlation scales - lma_adjust_ag
imgdtt_params.lma_adjust_wy, // boolean adjust_ellipse, // allow non-circular correlation maximums lma_adjust_wy
imgdtt_params.lma_adjust_wxy, // boolean adjust_lazyeye, // adjust disparity corrections and orthogonal disparities lma_adjust_wxy
xcenter, // double disp0, // initial value of disparity
imgdtt_params.lma_half_width, // double half_width, // A=1/(half_widh)^2 lma_half_width
imgdtt_params.lma_cost_wy // double cost_lazyeye // cost for each of the non-zero disparity corrections and ortho disparity lma_cost_wy
);
lma.setMatrices(disp_dist);
lma.initMatrices(); // should be called after initVector and after setMatrices
boolean lmaSuccess = false;
if (debug_level > 1) {
System.out.println("Input data:");
lma.printInputDataFx(false);
}
lmaSuccess = lma.runLma(
imgdtt_params.lma_lambda_initial, // double lambda, // 0.1
imgdtt_params.lma_lambda_scale_good, // double lambda_scale_good,// 0.5
imgdtt_params.lma_lambda_scale_bad, // double lambda_scale_bad, // 8.0
imgdtt_params.lma_lambda_max, // double lambda_max, // 100
imgdtt_params.lma_rms_diff, // double rms_diff, // 0.001
imgdtt_params.lma_num_iter, // int num_iter, // 20
debug_level); // int debug_level)
lma.updateFromVector();
double [] rms = lma.getRMS();
if (debug_level > 0) {
System.out.println("LMA ->"+lmaSuccess+" RMS="+rms[0]+", pure RMS="+rms[1]);
lma.printParams();
}
if (debug_level > 1) {
System.out.println("Input data and approximation:");
lma.printInputDataFx(true);
}
return lmaSuccess? lma: null;
}
public Correlations2dLMA corrLMA( // USED in lwir
ImageDttParameters imgdtt_params,
......@@ -1824,7 +1921,6 @@ public class Correlation2d {
lma, // Correlations2dLMA lma,
imgdtt_params.cnvx_add3x3, // boolean add3x3,
imgdtt_params.cnvx_weight, // double nc_cost,
debug_level); // int debug_level
boolean lmaSuccess;
if (run_poly_instead) { // not used in lwir
......
......@@ -2336,7 +2336,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
* @param rots misalignment correction (now includes zoom in addition to rotations
* @param deriv_rots derivatives by d_az, f_elev, d_rot, d_zoom
* @param pXYderiv - null or double[2 * number_of_cameras][] array to accommodate derivatives of px, py by each of the parameters
* @param disp_dist - null or double[2 * number_of_cameras][] array to accommodate X,Y derivatives by disp and CCW90 of disp
* @param disp_dist - null or double[number_of_cameras][4] array to accommodate X,Y (rows) derivatives by disp and CCW90 of disp (cols)
* @param px pixel X coordinate
* @param py pixel Y coordinate
* @param disparity disparity (for non-distorted image space)
......@@ -2452,8 +2452,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
}
if (disp_dist != null) {
disp_dist[2 * i] = new double [2]; // dx/d_disp, dx_d_ccw_disp
disp_dist[2 * i+1] = new double [2]; // dy/d_disp, dy_d_ccw_disp
disp_dist[i] = new double [4]; // dx/d_disp, dx_d_ccw_disp
// Not clear - what should be in Z direction before rotation here?
double [][] add0 = {
{-rXY[i][0], rXY[i][1], 0.0},
......@@ -2484,10 +2483,10 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
Matrix dd2 = rot2.transpose().times(scale_distort).times(rot2).times(dd1);
disp_dist[2 * i ][0] = dd2.get(0, 0);
disp_dist[2 * i ][1] = dd2.get(0, 1);
disp_dist[2 * i+1][0] = dd2.get(1, 0);
disp_dist[2 * i+1][1] = dd2.get(1, 1);
disp_dist[i][0] = dd2.get(0, 0);
disp_dist[i][1] = dd2.get(0, 1);
disp_dist[i][2] = dd2.get(1, 0);
disp_dist[i][3] = dd2.get(1, 1);
}
......@@ -3008,7 +3007,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
break; // too high distortion
}
if (Math.abs(rD-rDist)<delta) {
System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
if (debugThis) System.out.println(i+": "+iteration+" "+ Math.abs(rD-rDist)+" drDistDr="+drDistDr);
break; // success
}
r+=(rDist-rD)/drDistDr;
......
......@@ -1825,7 +1825,7 @@ public class ImageDtt {
centerY = tileY * transform_size + transform_size/2 - shiftY;
// TODO: move port coordinates out of color channel loop
double [][] centersXY;
double [][] disp_dist = new double[2 * quad][]; // used to correct 3D correlations
double [][] disp_dist = new double[quad][]; // used to correct 3D correlations
if ((disparity_array == null) || (disparity_array[tileY] == null) || (Double.isNaN(disparity_array[tileY][tileX]))) {
System.out.println("Bug with disparity_array !!!");
......@@ -1891,10 +1891,10 @@ public class ImageDtt {
}
// save disparity distortions for visualization:
for (int cam = 0; cam <quad; cam++) {
dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[ 2* cam + 0][0];
dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[ 2* cam + 0][1];
dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[ 2* cam + 1][0];
dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[ 2* cam + 1][1];
dbg_distort[cam * 4 + 0 ][nTile] = disp_dist[cam][0];
dbg_distort[cam * 4 + 1 ][nTile] = disp_dist[cam][1];
dbg_distort[cam * 4 + 2 ][nTile] = disp_dist[cam][2];
dbg_distort[cam * 4 + 3 ][nTile] = disp_dist[cam][3];
}
// TODO: use correction after disparity applied (to work for large disparity values)
......@@ -2156,6 +2156,7 @@ public class ImageDtt {
col_weights, // double [] col_weights,
corr_fat_zero); // double fat_zero)
// calculate interpolated "strips" to match different scales and orientations (ortho/diagonal) on the
// fine (0.5 pix) grid. ortho for scale == 1 provide even/even samples (1/4 of all), diagonal ones -
// checkerboard pattern
......@@ -2295,6 +2296,26 @@ public class ImageDtt {
if (tile_lma_debug_level > 0) {
System.out.println("Will run getMaxXSOrtho( ) for tileX="+tileX+", tileY="+tileY);
}
// debug new LMA correlations
if (debugTile) {
System.out.println("Will run new LMA for tileX="+tileX+", tileY="+tileY);
Corr2dLMA lma2 = corr2d.corrLMA2(
imgdtt_params, // ImageDttParameters imgdtt_params,
corrs, // double [][] corrs,
disp_dist,
imgdtt_params.dbg_pair_mask, // int pair_mask, // which pairs to process
false, // boolean run_poly_instead, // true - run LMA, false - run 2d polynomial approximation
0.5, // double sigma, // low-pass sigma to find maximum (and convex too
corr_stat[0], // double xcenter, // preliminary center x in pixels for largest baseline
imgdtt_params.ortho_vasw_pwr, // double vasw_pwr, // value as weight to this power,
tile_lma_debug_level, // int debug_level,
tileX, // int tileX, // just for debug output
tileY ); // int tileY
}
// disparity_map[DISPARITY_INDEX_CM + 1][tIndex] = // y not available here
// calculate/fill out hor and vert
// convert to multi-baseline combining results from several integer scales
......@@ -8246,8 +8267,8 @@ public class ImageDtt {
if (disparity_bimap != null){
disparity_bimap[BI_TARGET_INDEX][tIndex] = disparity_main;
}
double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[2 * quad_aux][]; // used to correct 3D correlations
double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[quad_aux][]; // used to correct 3D correlations
centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(
geometryCorrection_main, // GeometryCorrection gc_main,
......@@ -9049,8 +9070,8 @@ public class ImageDtt {
if (disparity_bimap != null){
disparity_bimap[BI_TARGET_INDEX][tIndex] = disparity_main;
}
double [][] disp_dist_main = new double[2 * quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[2 * quad_aux][]; // used to correct 3D correlations
double [][] disp_dist_main = new double[quad_main][]; // used to correct 3D correlations
double [][] disp_dist_aux = new double[quad_aux][]; // used to correct 3D correlations
if (calc_main) {// not used in lwir
centersXY_main = geometryCorrection_main.getPortsCoordinatesAndDerivatives(
geometryCorrection_main, // GeometryCorrection gc_main,
......
......@@ -94,10 +94,13 @@ public class ImageDttParameters {
public double corr_wndx_blur = 5.0; // 100% to 0 % vertical transition range
// LMA parameters
public boolean lma_adjust_wm = true;
public boolean lma_adjust_wy = false;
public boolean lma_adjust_wxy = true;
public boolean lma_adjust_ag = true;
public boolean lma_adjust_wm = true; // used in new for width
public boolean lma_adjust_wy = false; // used in new for ellipse
public boolean lma_adjust_wxy = true; // used in new for lazy eye
public boolean lma_adjust_ag = true;// used in new for gains
// new LMA parameters
public double lma_half_width = 2.0; //
public double lma_cost_wy = 0.1; //
......@@ -241,12 +244,12 @@ public class ImageDttParameters {
gd.addTab("Corr LMA","Parameters for LMA fitting of the correlation maximum parameters");
gd.addCheckbox ("Fit correlation defined half-width", this.lma_adjust_wm,
"Allow fitting of the half-width common for all pairs, defined by the LPF filter of the phase correlation");
gd.addCheckbox ("Fit extra vertical half-width", this.lma_adjust_wy,
"Fit extra perpendicular to disparity half-width (not used? and only possible with multi-baseline cameras)");
gd.addCheckbox ("Fit extra half-width along disparity direction", this.lma_adjust_wxy,
gd.addCheckbox ("Adjust ellipse parameters (was Fit extra vertical half-width)", this.lma_adjust_wy,
"Adjust ellipse (non-circular) of the correlation maximum (was Fit extra perpendicular to disparity half-width (not used? and only possible with multi-baseline cameras))");
gd.addCheckbox ("Adjust \"lazy eye\" paramdeters (was Fit extra half-width along disparity direction)", this.lma_adjust_wxy,
"Increased width in disparity direction caused by multi-distance objects in the tile");
gd.addCheckbox ("Adjust per-group amplitudes", this.lma_adjust_ag,
"Each correlation type's amplitude (now always needed)");
gd.addCheckbox ("Adjust per-pair scale (was Adjust per-group amplitudes)", this.lma_adjust_ag,
"Each correlation pair gain (was Each correlation type's amplitude (now always needed))");
gd.addNumericField("Initial/expected half-width of the correlation maximum in both directions", this.lma_half_width, 3, 6, "pix",
"With LPF sigma = 0.9 it seems to be ~= 2.0. Used both as initial parameter and the fitted value difference from this may be penalized");
......
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