Skip to content

I
imagej-elphel
Commit 0a050e16 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

extrinsic corrections

parent fabeb071
Hide whitespace changes
Inline Side-by-side
Showing with 816 additions and 123 deletions
src/main/java/AlignmentCorrection.java
View file @ 0a050e16
...@@ -45,13 +45,24 @@ public class AlignmentCorrection { ...@@ -45,13 +45,24 @@ public class AlignmentCorrection {
} }
} }
/*
static double [][]dMismatch_dXY = {
{ 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }, // mv0 = dy0
{ 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 , 0.0 }, // mv1 = dy1
{ 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 }, // mv2 = dx2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 }, // mv3 = dx3
{-0.5 , 0.0 , 0.5 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }, // mv4 = (dx1 - dx0)/2;
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , -0.5 , 0.0 , 0.5 }, // mv5 = (dy3 - dy2)/2;
{ 0.25, 0.0 , 0.25, 0.0 , 0.0 , -0.25, 0.0 ,-0.25 }, // mv6 = (dx0 + dx1 -dy2 - dy3)/4;
{ 0.125, 0.0 , 0.125, 0.0 , 0.0 , 0.125, 0.0 , 0.125}}; // mv6 = (dx0 + dx1 +dy2 + dy3)/8;
*/
public class Mismatch{ public class Mismatch{
public boolean use_disparity; // adjust dx0+dx1+dy0+dy1 == 0
public double [] pXY; // tile center x,y public double [] pXY; // tile center x,y
public double disparity_task; public double disparity_task;
public double disparity_meas; public double disparity_meas;
public double strength; public double strength;
public double [] offsets; public double [][] offsets; // measured {{dx0,dy0}, {dx1,dy1},{dx2,dy2},{dx3,dy3}};
public Mismatch() public Mismatch()
{ {
...@@ -59,39 +70,28 @@ public class AlignmentCorrection { ...@@ -59,39 +70,28 @@ public class AlignmentCorrection {
disparity_task = 0.0; disparity_task = 0.0;
disparity_meas = 0.0; disparity_meas = 0.0;
strength = 0.0; strength = 0.0;
offsets = new double[2*NUM_SENSORS]; offsets = new double[NUM_SENSORS][2];
} }
public Mismatch ( public Mismatch (
boolean use_disparity, // adjust dx0+dx1+dy0+dy1 == 0
double [] pXY, // tile center x,y double [] pXY, // tile center x,y
double disparity_task, double disparity_task,
double disparity_meas, double disparity_meas,
double strength, double strength,
double [] offsets ) double [][] offsets )
{ {
this.use_disparity = use_disparity;
this.pXY = pXY; this.pXY = pXY;
this.disparity_task = disparity_task; this.disparity_task = disparity_task; // currently wrong, series is 0/1, not measurement number
this.disparity_meas = disparity_meas; this.disparity_meas = disparity_meas;
this.strength = strength; this.strength = strength;
this.offsets = offsets; this.offsets = offsets;
} }
public Mismatch (
double [] pXY, // tile center x,y public boolean usesDisparity()
double disparity_task,
double disparity_meas,
double strength,
double [][] offsets )
{ {
this.pXY = pXY; return use_disparity;
this.disparity_task = disparity_task;
this.disparity_meas = disparity_meas;
this.strength = strength;
this.offsets = new double [NUM_SENSORS * 2];
for (int i = 0; i < NUM_SENSORS; i++){
this.offsets[i*2 + 0] = offsets[i][0];
this.offsets[i*2 + 1] = offsets[i][1];
}
} }
public double [] getPXY(){ public double [] getPXY(){
return pXY; return pXY;
} }
...@@ -105,21 +105,92 @@ public class AlignmentCorrection { ...@@ -105,21 +105,92 @@ public class AlignmentCorrection {
public double getStrength() { public double getStrength() {
return strength; return strength;
} }
public double [] getOffsets(){
double [] offsets1d = new double [offsets.length * offsets[0].length];
for (int i = 0; i < offsets.length; i++){
for (int j = 0; j < offsets[i].length; j++){
offsets1d[i * offsets[i].length + j] = offsets[i][j];
}
}
return offsets1d;
}
/**
* Calculate error vector of 8 components, the last one will be optionally masked by weight - used only when measured
* disparity should be 0 (as at infinity)
* @return 8 components
*/
public double [] getY()
{
double [] y = {
offsets[0][1], //dy0;
offsets[1][1], //dy1;
offsets[2][0], //dx2;
offsets[3][0], //dx3;
(offsets[1][0] - offsets[0][0]), //(dx1 - dx0)/2; 0.5/magic is already applied
(offsets[3][1] - offsets[2][1]), //(dy3 - dy2)/2;
0.5* (offsets[0][0] + offsets[1][0] - offsets[2][1] - offsets[3][1]), //(dx0 + dx1 -dy2 - dy3)/4;
use_disparity ? (0.25* (offsets[0][0] + offsets[1][0] + offsets[2][1] + offsets[3][1])) : 0.0 // only when disparity is known to be 0
};
return y;
}
public void copyToY( public void copyToY(
double [] y, double [] y,
int n_sample) int n_sample)
{ {
System.arraycopy(this.offsets, 0, y, n_sample * (2 * NUM_SENSORS), (2 * NUM_SENSORS)); double [] sub_y = getY();
System.arraycopy(sub_y, 0, y, n_sample * (2 * NUM_SENSORS), (2 * NUM_SENSORS));
} }
public void copyToW( public void copyToW(
double [] w, double [] w,
int n_sample) int n_sample)
{ {
for (int i = n_sample * (2 * NUM_SENSORS); i < (n_sample + 1) * (2 * NUM_SENSORS); i++){ // for (int i = n_sample * (2 * NUM_SENSORS); i < (n_sample + 1) * (2 * NUM_SENSORS); i++){
w[i] = strength; for (int i = 0; i < 2 * NUM_SENSORS; i++){
w[n_sample * (2 * NUM_SENSORS) + i] = (use_disparity || (i < 7)) ? strength : 0.0;
}
}
/**
* Convert transposed jacobian from {d_dx0,d_dy0, ...,d_dy3} to d_mvi (measurement vectors),
* where sum of measurement vectors squared is minimized. Same matrix multiplications
* is applied to each group of 8 columns. last column in each group is only non-zero if
* disparity is known to be 0;
* @param jt transposed Jacobian of 10/9 rows and 8*n columns
* @return converted transposed Jacobian of the same dimensions
*/
double [][] convertJt_mv(
double [][] jt)
{
double [][] dMismatch_dXY = { // extra 0.5 is because differences dxi, dyi are already *= 0.5/magic
//x0 y0 x1 y1 x2 y2 x3 y3
{ 0.0 , -0.5, 0.0 , 0.5 , 0.0 , 0.0 , 0.0 , 0.0 }, // mv0 = dy0 = y1 - y0
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , -0.5 , 0.0 , 0.5 }, // mv1 = dy1 = y3 - y2
{-0.5 , 0.0 , 0.0 , 0.0 , 0.5 , 0.0 , 0.0 , 0.0 }, // mv2 = dx2 = x2 - x0
{ 0.0 , 0.0 , -0.5 , 0.0 , 0.0 , 0.0 , 0.5 , 0.0 }, // mv3 = dx3 = x3 - x1
{-0.25, 0.0 , 0.25, 0.0 , -0.25, 0.0 , 0.25, 0.0 }, // mv4 = (dx1 - dx0)/2 = (x3 - x2 - x0 + x1) / 2
{ 0.0 , -0.25, 0.0 , -0.25, 0.0 , 0.25, 0.0 , 0.25 }, // mv5 = (dy3 - dy2)/2 = (y3 - y1 - y0 + y2) / 2
{-0.125, 0.125, 0.125, 0.125, -0.125, -0.125, 0.125, -0.125 }, // mv6 = (dx0 + dx1 -dy2 - dy3)/4 = (x1 - x0 + x3 - x2 - y2 + y0 - y3 + y1)/4
{-0.0625, -0.0625, 0.0625,-0.0625,-0.0625, 0.0625, 0.0625, 0.0625}};// mv7 = (dx0 + dx1 +dy2 + dy3)/8= (x1 - x0 + x3 - x2 + y2 - y0 + y3 - y1)/8
double [][] jt_conv = new double [jt.length][jt[0].length/dMismatch_dXY[0].length*dMismatch_dXY.length]; // now dMismatch_dXY is square
// multiplying by transposed dMismatch_dXY
for (int g = 0; g < jt[0].length/dMismatch_dXY[0].length; g++) {
int indx_in = dMismatch_dXY[0].length * g;
int indx_out = dMismatch_dXY.length * g;
for (int np = 0; np < jt.length; np++){
for (int oindx = 0; oindx < dMismatch_dXY.length; oindx++){
for (int k = 0; k < dMismatch_dXY[0].length; k++) {
jt_conv[np][indx_out + oindx] += jt[np][indx_in + k] * dMismatch_dXY[oindx][k];
}
}
}
} }
return jt_conv;
} }
} }
//System.arraycopy(dpixels, (tileY*width+tileX)*dct_size + i*width, tile_in, i*n2, n2); //System.arraycopy(dpixels, (tileY*width+tileX)*dct_size + i*width, tile_in, i*n2, n2);
...@@ -713,10 +784,11 @@ public class AlignmentCorrection { ...@@ -713,10 +784,11 @@ public class AlignmentCorrection {
double [] xy = new double[8]; // same as coefficients: x0,y0,x1,y1,x2,y2,x3,y3 double [] xy = new double[8]; // same as coefficients: x0,y0,x1,y1,x2,y2,x3,y3
// Calculate x0,x1,x2,x3 and y0,y1,y2,y3 assuming x0+x1+x2+x3 = 0,y0+y1+y2+y3 = 0 and minimizing squares of errors // Calculate x0,x1,x2,x3 and y0,y1,y2,y3 assuming x0+x1+x2+x3 = 0,y0+y1+y2+y3 = 0 and minimizing squares of errors
// as each each 4: "dx0", "dx1", "dx2", "dx3" and "dy0", "dy1", "dy2", "dy3" are over-defined // as each each 4: "dx0", "dx1", "dx2", "dx3" and "dy0", "dy1", "dy2", "dy3" are over-defined
double [][] dxy = new double[4][2];
for (int dir = 0; dir < 2; dir++) { // 0 - X, 1 - Y for (int dir = 0; dir < 2; dir++) { // 0 - X, 1 - Y
double [] dxy = new double[4]; // double [] dxy = new double[4];
for (int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++){
dxy[i] = scale * disp_strength[indices_mismatch[dir][i] + (s.series * NUM_SLICES)][s.tile]; dxy[i][dir] = scale * disp_strength[indices_mismatch[dir][i] + (s.series * NUM_SLICES)][s.tile];
} }
/* /*
...@@ -803,9 +875,9 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -803,9 +875,9 @@ B = |+dy0 -dy1 -2*dy3 |
*/ */
double [] B_arr = { double [] B_arr = {
-dxy[0] -dxy[1] -dxy[2] -dxy[3], -dxy[0][dir] -dxy[1][dir] -dxy[2][dir] -dxy[3][dir],
dxy[0] -dxy[1] -2 * dxy[3], dxy[0][dir] -dxy[1][dir] -2 * dxy[3][dir],
dxy[2] -2 * dxy[1] -dxy[3]}; dxy[2][dir] -2 * dxy[1][dir] -dxy[3][dir]};
Matrix B = new Matrix(B_arr, 3); // 3 rows Matrix B = new Matrix(B_arr, 3); // 3 rows
Matrix X = AINV.times(B); Matrix X = AINV.times(B);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
...@@ -861,11 +933,12 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -861,11 +933,12 @@ B = |+dy0 -dy1 -2*dy3 |
// final int disp_scan_count, // final int disp_scan_count,
mismatch_list.add(new Mismatch( mismatch_list.add(new Mismatch(
false, // public boolean use_disparity; // adjust dx0+dx1+dy0+dy1 == 0
centerXY, centerXY,
disparity_task, disparity_task,
disparity_meas, disparity_meas,
strength, strength,
xy)); dxy)); // xy));
} }
indx ++; indx ++;
} }
...@@ -1892,13 +1965,15 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -1892,13 +1965,15 @@ B = |+dy0 -dy1 -2*dy3 |
"mismatch_corr_coefficiants"); "mismatch_corr_coefficiants");
} }
} }
if (mismatch_list != null){ if (!use_poly && (mismatch_list != null)){
boolean apply_extrinsic = true; boolean apply_extrinsic = true;
GeometryCorrection.CorrVector corr_vector = solveCorr ( GeometryCorrection.CorrVector corr_vector = solveCorr (
clt_parameters.ly_inf_en, // boolean use_disparity, // if true will ignore disparity data even if available (was false)
clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
mismatch_list, // ArrayList<Mismatch> mismatch_list, mismatch_list, // ArrayList<Mismatch> mismatch_list,
qc.geometryCorrection, // GeometryCorrection geometryCorrection, qc.geometryCorrection, // GeometryCorrection geometryCorrection,
qc.geometryCorrection.getCorrVector(), // GeometryCorrection.CorrVector corr_vector, qc.geometryCorrection.getCorrVector(), // GeometryCorrection.CorrVector corr_vector,
1); // int debugLevel) 2); // 1); // int debugLevel)
if (debugLevel > -1){ if (debugLevel > -1){
System.out.println("Old extrinsic corrections:"); System.out.println("Old extrinsic corrections:");
System.out.println(qc.geometryCorrection.getCorrVector().toString()); System.out.println(qc.geometryCorrection.getCorrVector().toString());
...@@ -1912,6 +1987,12 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -1912,6 +1987,12 @@ B = |+dy0 -dy1 -2*dy3 |
System.out.println(qc.geometryCorrection.getCorrVector().toString()); System.out.println(qc.geometryCorrection.getCorrVector().toString());
} }
} }
} else {
if (debugLevel > -1){
System.out.println("Extrinsic parameters (tilt, azimuth, roll) of subcameras is disabled, clt_parameters.ly_poly="+
clt_parameters.ly_poly+" (should be false for extrinsics)");
System.out.println(qc.geometryCorrection.getCorrVector().toString());
}
} }
return mismatch_corr_coefficiants; return mismatch_corr_coefficiants;
} }
...@@ -1995,66 +2076,225 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -1995,66 +2076,225 @@ B = |+dy0 -dy1 -2*dy3 |
} }
double [][] getJacobianTransposed( double [][] getJacobianTransposed(
boolean [] par_mask,
ArrayList<Mismatch> mismatch_list, ArrayList<Mismatch> mismatch_list,
GeometryCorrection geometryCorrection, GeometryCorrection geometryCorrection,
GeometryCorrection.CorrVector corr_vector, GeometryCorrection.CorrVector corr_vector,
int debugLevel) int debugLevel)
{ {
double [][] jt = new double [GeometryCorrection.CorrVector.LENGTH][2 * NUM_SENSORS * mismatch_list.size()]; boolean dbg_images = debugLevel>1;
int dbg_decimate = 64; // just for the debug image
int dbg_width = qc.tp.getTilesX()*qc.tp.getTileSize();
int dbg_height = qc.tp.getTilesY()*qc.tp.getTileSize();
int dbg_owidth = dbg_width/dbg_decimate;
int dbg_oheight = dbg_height/dbg_decimate;
int dbg_length = dbg_owidth*dbg_oheight;
String [] dbg_titles_tar=GeometryCorrection.CORR_NAMES;
String [] dbg_titles_sym= {"sym0","sym1","sym2","sym3","sym4","sym5","sroll0","sroll1","sroll2","sroll3"};
String [] dbg_titles_xy= {"x0","y0","x1","y1","x2","y2","x3","y3"};
// String [] dbg_titles_mv= {"dy0","dy1","dx2","dx3","dx1-dx0","dy3-dy2","dh-dv","dhy+dv"};
double [][] dbg_img_deriv = null; // compare derivatives with delta-diffs
// double [][] dbg_xy = null; // jacobian dmv/dsym
// double [][] dbg_mv = null; // jacobian dmv/dsym
double [][] dbg_dxy_dsym = null; // jacobian dxy/dsym
// double [][] dbg_dmv_dsym = null; // jacobian dmv/dsym
if (dbg_images) {
dbg_img_deriv = doubleNaN(dbg_titles_xy.length * dbg_titles_tar.length *2, dbg_length); // compare derivatives with delta-diffs
// dbg_xy = doubleNaN(dbg_titles_xy.length, dbg_length); // jacobian dmv/dsym
// dbg_mv = doubleNaN(dbg_titles_mv.length, dbg_length); // jacobian dmv/dsym
dbg_dxy_dsym = doubleNaN(dbg_titles_xy.length * dbg_titles_sym.length, dbg_length); // jacobian dxy/dsym
// dbg_dmv_dsym = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym
}
int num_pars = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) num_pars ++;
double [][] jt = new double [num_pars][2 * NUM_SENSORS * mismatch_list.size()];
double [][] jt_dbg = null; double [][] jt_dbg = null;
if (debugLevel > 0){ if (debugLevel > 0){
jt_dbg = new double [GeometryCorrection.CorrVector.LENGTH][2 * NUM_SENSORS * mismatch_list.size()]; jt_dbg = new double [num_pars][2 * NUM_SENSORS * mismatch_list.size()];
} }
for (int indx = 0; indx<mismatch_list.size(); indx++){ // need indx value for (int indx = 0; indx<mismatch_list.size(); indx++){ // need indx value
Mismatch mm = mismatch_list.get(indx); Mismatch mm = mismatch_list.get(indx);
double [] pXY = mm.getPXY(); double [] pXY = mm.getPXY();
double [][][][] fj = geometryCorrection.getPortsCoordinatesAndDerivatives( double [][] deriv = new double [2 * NUM_SENSORS][];
int dbg_index =dbg_index (pXY, dbg_decimate);
// double [][] f =
geometryCorrection.getPortsCoordinatesAndDerivatives(
corr_vector, // CorrVector corr_vector, corr_vector, // CorrVector corr_vector,
true, // boolean calc_deriv, deriv, // boolean calc_deriv,
pXY[0], // double px, pXY[0], // double px,
pXY[1], // double py, pXY[1], // double py,
mm.getDisparityTask()); // double disparity) mm.getDisparityMeas()); // getDisparityTask()); // double disparity)
// convert to symmetrical coordianets
double [][] jt_partial = corr_vector.getJtPartial(
deriv, // double [][] port_coord_deriv,
par_mask); // boolean [] par_mask
// put partial transposed jacobian into full transposed Jacobian
for (int npar = 0; npar < jt.length; npar++){ for (int npar = 0; npar < jt.length; npar++){
for (int ns = 0; ns < NUM_SENSORS; ns++){ for (int n = 0; n < 2* NUM_SENSORS; n++){
for (int dir = 0; dir < 2; dir++){ // jt[npar][2 * NUM_SENSORS * indx + n] = j_partial[n][npar]; // here Jacobian was not transposed
jt[npar][2 * NUM_SENSORS * indx + 2 * ns + dir] = fj[1][ns][dir][npar]; jt[npar][2 * NUM_SENSORS * indx + n] = jt_partial[npar][n];
}
} }
} }
if (debugLevel > 0){ if (debugLevel > 0){
double [][][][] fj_dbg = null; // double [][] j_partial_debug = new double [2 * NUM_SENSORS][];
fj_dbg = geometryCorrection.getPortsCoordinatesAndDerivatives( double [][] deriv_dbg = new double [2 * NUM_SENSORS][];
1E-6, // double delta, // 1e-6 double [] dbg_a_vector= null;
/*
double [] dbg_a_vector= {
0.0038591302038724394,
-0.08463081841166764,
0.06130822266181911,
-0.036393168371534744,
0.025155872946661495,
0.0,
0.0
};
double [] dbg_a_vector= {
0.5, // 0.0038591302038724394, // 0.0, //
0.0, // -0.08463081841166764,
0.0, // 0.06130822266181911,
0.0, // -0.036393168371534744,
0.0, // 0.025155872946661495,
0.0,
0.0
};
*/
geometryCorrection.getPortsCoordinatesAndDerivatives(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
1E-8, //6, // double delta, // 1e-6
corr_vector, // CorrVector corr_vector, corr_vector, // CorrVector corr_vector,
deriv_dbg, // j_partial_debug, //
pXY[0], // double px, pXY[0], // double px,
pXY[1], // double py, pXY[1], // double py,
mm.getDisparityTask()); // double disparity) mm.getDisparityMeas()); // Task()); // double disparity)
// convert to symmetrical coordinates
double [][] jt_partial_dbg = corr_vector.getJtPartial(
deriv_dbg, // double [][] port_coord_deriv,
par_mask); // boolean [] par_mask
double max_rdiff = 0; double max_rdiff = 0;
for (int npar = 0; npar < jt.length; npar++){ for (int npar = 0; npar < jt.length; npar++){
for (int ns = 0; ns < NUM_SENSORS; ns++){ for (int n = 0; n < 2 * NUM_SENSORS; n++){
for (int dir = 0; dir < 2; dir++){ // jt_dbg[npar][2 * NUM_SENSORS * indx + n] = j_partial_debug[n][npar]; // here Jacobian was not transposed
jt_dbg[npar][2 * NUM_SENSORS * indx + 2 * ns + dir] = fj_dbg[1][ns][dir][npar]; jt_dbg[npar][2 * NUM_SENSORS * indx + n] = jt_partial_dbg[npar][n];
double avg = 0.5*(jt_dbg[npar][2 * NUM_SENSORS * indx + 2 * ns + dir] + jt[npar][2 * NUM_SENSORS * indx + 2 * ns + dir]); double avg = 0.5*(jt_dbg[npar][2 * NUM_SENSORS * indx + n] + jt[npar][2 * NUM_SENSORS * indx + n]);
double rdiff= Math.abs(0.5*(jt_dbg[npar][2 * NUM_SENSORS * indx + 2 * ns + dir] - jt[npar][2 * NUM_SENSORS * indx + 2 * ns + dir])); double rdiff= Math.abs(0.5*(jt_dbg[npar][2 * NUM_SENSORS * indx + n] - jt[npar][2 * NUM_SENSORS * indx + n]));
if (avg != 0.0){ if (avg != 0.0){
rdiff /= avg; rdiff /= avg;
} }
if (rdiff > max_rdiff){ if (rdiff > max_rdiff){
max_rdiff = rdiff; max_rdiff = rdiff;
}
} }
} }
} }
if (debugLevel > 1) { if (debugLevel > 2) {
System.out.println(String.format("px = %5.0f py = %5.0f disp_task = %7.3f disp_meas = %7.3f strength = %7.3f max rdiff = %f", System.out.println(String.format("px = %5.0f py = %5.0f disp_task = %7.3f disp_meas = %7.3f strength = %7.3f max rdiff = %f",
mm.getPXY()[0], mm.getPXY()[1], mm.getDisparityTask(), mm.getDisparityMeas(), mm.getStrength(), max_rdiff)); mm.getPXY()[0], mm.getPXY()[1], mm.getDisparityTask(), mm.getDisparityMeas(), mm.getStrength(), max_rdiff));
} }
if (dbg_images) {
for (int i = 0; i < dbg_titles_xy.length; i++){
for (int j = 0; j < dbg_titles_tar.length; j++){
dbg_img_deriv[2 * (i * dbg_titles_tar.length + j) + 0][dbg_index] = deriv[i][j];
dbg_img_deriv[2 * (i * dbg_titles_tar.length + j) + 1][dbg_index] = deriv_dbg[i][j];
}
}
for (int i = 0; i < dbg_titles_xy.length; i++){
int oj = 0;
for (int j = 0; j < dbg_titles_sym.length; j++) if (par_mask[j]){
dbg_dxy_dsym[i * dbg_titles_sym.length + j][dbg_index] = jt_partial[oj][i];
oj++;
}
}
}
}
}
if (dbg_images) {
String [] dbg_img_deriv_titles = new String [dbg_titles_xy.length * dbg_titles_tar.length *2];
for (int i = 0; i < dbg_titles_xy.length; i++){
for (int j = 0; j < dbg_titles_tar.length; j++){
dbg_img_deriv_titles[2 * (i * dbg_titles_tar.length + j) + 0]= dbg_titles_xy[i] + "_" +dbg_titles_tar[j];
dbg_img_deriv_titles[2 * (i * dbg_titles_tar.length + j) + 1]= dbg_titles_xy[i] + "_" +dbg_titles_tar[j] + "delta";
}
}
// dbg_xy = new double [dbg_titles_xy.length] [dbg_length]; // jacobian dmv/dsym
// dbg_mv = new double [dbg_titles_mv.length] [dbg_length]; // jacobian dmv/dsym
String [] dbg_dxy_dsym_titles = new String [dbg_titles_xy.length * dbg_titles_sym.length];
for (int i = 0; i < dbg_titles_xy.length; i++){
for (int j = 0; j < dbg_titles_sym.length; j++){
dbg_dxy_dsym_titles[i * dbg_titles_sym.length + j]= dbg_titles_xy[i] + "_" +dbg_titles_sym[j];
}
} }
// String [] dmv_dmv_dsym_titles = new String [dbg_titles_mv.length * dbg_titles_sym.length];
// for (int i = 0; i < dbg_titles_mv.length; i++){
// for (int j = 0; j < dbg_titles_sym.length; j++){
// dmv_dmv_dsym_titles[i * dbg_titles_sym.length + j]= dbg_titles_mv[i] + "_" +dbg_titles_sym[j];
// }
// }
dbgImgRemoveEmpty(dbg_img_deriv);
// dbgImgRemoveEmpty(dbg_xy);
// dbgImgRemoveEmpty(dbg_mv);
dbgImgRemoveEmpty(dbg_dxy_dsym);
// dbgImgRemoveEmpty(dbg_dmv_dsym);
showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
sdfa_instance.showArrays(dbg_img_deriv, dbg_owidth, dbg_oheight, true, "dbg_img_deriv", dbg_img_deriv_titles);
// sdfa_instance.showArrays(dbg_xy, dbg_owidth, dbg_oheight, true, "dbg_xy", dbg_titles_xy);
// sdfa_instance.showArrays(dbg_mv, dbg_owidth, dbg_oheight, true, "dbg_mv", dbg_titles_mv);
sdfa_instance.showArrays(dbg_dxy_dsym, dbg_owidth, dbg_oheight, true, "dbg_dxy_dsym", dbg_dxy_dsym_titles);
// sdfa_instance.showArrays(dbg_dmv_dsym, dbg_owidth, dbg_oheight, true, "dbg_dmv_dsym", dmv_dmv_dsym_titles);
} }
return jt; return jt;
} }
public int dbg_index(double [] pXY, int decimate)
{
int width = qc.tp.getTilesX()*qc.tp.getTileSize()/decimate;
int height = qc.tp.getTilesY()*qc.tp.getTileSize()/decimate;
int x = (int) Math.round(pXY[0]/decimate);
int y = (int) Math.round(pXY[1]/decimate);
if (x < 0) x = 0;
else if (x >= width) x = width - 1;
if (y < 0) y = 0;
else if (y >= height) y = height - 1;
return x + width * y;
}
public double [][] doubleNaN(int layers, int leng){
double [][] dbg_img = new double [layers][leng];
for (int nimg = 0; nimg < dbg_img.length; nimg++) if (dbg_img[nimg] != null){
for (int i = 0; i < dbg_img[nimg].length; i++){
dbg_img[nimg][i] = Double.NaN;
}
}
return dbg_img;
}
public void dbgImgRemoveEmpty(double [][] dbg_img){
for (int nimg = 0; nimg < dbg_img.length; nimg++) if (dbg_img[nimg] != null){
boolean has_data = false;
for (int i = 0; i < dbg_img[nimg].length; i++){
if (!Double.isNaN(dbg_img[nimg][i]) && (dbg_img[nimg][i] != 0.0)){
has_data = true;
break;
}
}
if (!has_data){
dbg_img[nimg] = null;
}
}
}
double [][] getJTJ( double [][] getJTJ(
double [][] jt, double [][] jt,
double [] w) double [] w)
...@@ -2062,7 +2302,7 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2062,7 +2302,7 @@ B = |+dy0 -dy1 -2*dy3 |
double [][] jtj = new double [jt.length][jt.length]; double [][] jtj = new double [jt.length][jt.length];
for (int i = 0; i < jt.length; i++){ for (int i = 0; i < jt.length; i++){
for (int j = 0; j < i; j++){ for (int j = 0; j < i; j++){
jt[i][j] = jt[j][i]; jtj[i][j] = jtj[j][i];
} }
for (int j = i; j < jt.length; j++){ for (int j = i; j < jt.length; j++){
for (int k = 0; k < jt[0].length; k++){ for (int k = 0; k < jt[0].length; k++){
...@@ -2125,19 +2365,41 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2125,19 +2365,41 @@ B = |+dy0 -dy1 -2*dy3 |
} }
public GeometryCorrection.CorrVector solveCorr ( public GeometryCorrection.CorrVector solveCorr (
boolean use_disparity, // if true will ignore disparity data even if available
boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
// data, using just radial distortions
ArrayList<Mismatch> mismatch_list, ArrayList<Mismatch> mismatch_list,
GeometryCorrection geometryCorrection, GeometryCorrection geometryCorrection,
GeometryCorrection.CorrVector corr_vector, GeometryCorrection.CorrVector corr_vector,
int debugLevel) int debugLevel)
{ {
boolean has_disparity = force_convergence; // force false;
// See if at least some data has disparities to be adjusted
if (use_disparity) {
for (Mismatch mm: mismatch_list){
if (mm.use_disparity) has_disparity = true;
break;
}
}
boolean [] par_mask = new boolean[10];
for (int i = (has_disparity ? 0 : 1); i < par_mask.length; i++){
par_mask[i] = true;
}
double [][] jta = getJacobianTransposed( double [][] jta = getJacobianTransposed(
par_mask, // boolean [] par_mask,
mismatch_list, // ArrayList<Mismatch> mismatch_list, mismatch_list, // ArrayList<Mismatch> mismatch_list,
geometryCorrection, // GeometryCorrection geometryCorrection, geometryCorrection, // GeometryCorrection geometryCorrection,
corr_vector, // GeometryCorrection.CorrVector corr_vector) corr_vector, // GeometryCorrection.CorrVector corr_vector)
debugLevel); // int debugLevel) debugLevel); // int debugLevel)
Matrix jt = new Matrix(jta); // convert Jacobian outputs to symmetrical measurement vectors (last one is non-zero only if disaprity should be adjusted)
double [] y_minus_fx_a = getYminusFx(
double [][] jta_mv = (new Mismatch()).convertJt_mv (jta); //double [][] jt)
Matrix jt = new Matrix(jta_mv);
double [] y_minus_fx_a = getYminusFx( // mv[0]..mv[7], not the measured data (dx0, dy0, ... dx3, dy3)
mismatch_list); // ArrayList<Mismatch> mismatch_list) mismatch_list); // ArrayList<Mismatch> mismatch_list)
double [] weights = getWeights( double [] weights = getWeights(
...@@ -2145,15 +2407,95 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2145,15 +2407,95 @@ B = |+dy0 -dy1 -2*dy3 |
double [] y_minus_fx_a_weighted = mulWeight(y_minus_fx_a, weights); double [] y_minus_fx_a_weighted = mulWeight(y_minus_fx_a, weights);
double rms0 = getRMS (y_minus_fx_a, weights); double rms0 = getRMS (y_minus_fx_a, weights);
if (debugLevel > -1){ if (debugLevel > -1){
System.out.println("solveCorr(): initial RMS = " + rms0); System.out.println("--- solveCorr(): initial RMS = " + rms0);
}
Matrix y_minus_fx_weighted = new Matrix(y_minus_fx_a_weighted, y_minus_fx_a_weighted.length);
double [][] jtja = getJTJ(jta, weights);
Matrix jtj = new Matrix(jtja); // getJTJ(jta, weights)); // less operations than jt.times(jt.transpose());
//
boolean dbg_images = debugLevel>1;
int dbg_decimate = 64; // just for the debug image
int dbg_width = qc.tp.getTilesX()*qc.tp.getTileSize();
int dbg_height = qc.tp.getTilesY()*qc.tp.getTileSize();
int dbg_owidth = dbg_width/dbg_decimate;
int dbg_oheight = dbg_height/dbg_decimate;
int dbg_length = dbg_owidth*dbg_oheight;
// String [] dbg_titles_tar=GeometryCorrection.CORR_NAMES;
String [] dbg_titles_sym= {"sym0","sym1","sym2","sym3","sym4","sym5","sroll0","sroll1","sroll2","sroll3"};
String [] dbg_titles_xy= {"x0","y0","x1","y1","x2","y2","x3","y3"};
String [] dbg_titles_mv= {"dy0","dy1","dx2","dx3","dx1-dx0","dy3-dy2","dh-dv","dhy+dv"};
double [][] dbg_xy = null; // jacobian dmv/dsym
double [][] dbg_mv = null; // jacobian dmv/dsym
double [][] dbg_dmv_dsym = null; // jacobian dmv/dsym
if (dbg_images) {
dbg_xy = doubleNaN(dbg_titles_xy.length, dbg_length); // jacobian dmv/dsym
dbg_mv = doubleNaN(dbg_titles_mv.length, dbg_length); // jacobian dmv/dsym
dbg_dmv_dsym = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym
// dbg_xy = new double [dbg_titles_xy.length] [dbg_length]; // jacobian dmv/dsym
// dbg_mv = new double [dbg_titles_mv.length] [dbg_length]; // jacobian dmv/dsym
String [] dbg_dmv_dsym_titles = new String [dbg_titles_mv.length * dbg_titles_sym.length];
for (int i = 0; i < dbg_titles_mv.length; i++){
for (int j = 0; j < dbg_titles_sym.length; j++){
dbg_dmv_dsym_titles[i * dbg_titles_sym.length + j]= dbg_titles_mv[i] + "_" +dbg_titles_sym[j];
}
}
for (int indx = 0; indx < mismatch_list.size(); indx++){
Mismatch mm = mismatch_list.get(indx);
double [] pXY = mm.getPXY();
int dbg_index =dbg_index (pXY, dbg_decimate);
double [] xy = mm.getOffsets();
double [] mv = mm.getY();
for (int i = 0; i < xy.length; i++){
dbg_xy[i][dbg_index] = xy[i];
}
for (int i = 0; i < mv.length; i++){
dbg_mv[i][dbg_index] = mv[i];
}
// Now Jacobian - get from full one
//System.out.println("solveCorr(): dbg_dmv_dsym.length="+dbg_dmv_dsym.length+ ", dbg_dmv_dsym[0].length="+dbg_dmv_dsym[0].length);
//System.out.println("solveCorr(): jta_mv.length="+jta_mv.length+", jta_mv[0].length="+jta_mv[0].length+" dbg_index="+dbg_index);
for (int i = 0; i < dbg_titles_mv.length; i++){
int oj = 0;
for (int j = 0; j < dbg_titles_sym.length; j++) if (par_mask[j]){
if ((i * dbg_titles_sym.length + j) >= dbg_dmv_dsym.length) {
System.out.println("solveCorr(): dbg_dmv_dsym.length="+dbg_dmv_dsym.length+ ", dbg_dmv_dsym[0].length="+dbg_dmv_dsym[0].length);
} else if ((dbg_titles_mv.length * indx + i) >= jta_mv [oj].length){
System.out.println("solveCorr(): dbg_dmv_dsym.length="+dbg_dmv_dsym.length+ ", dbg_dmv_dsym[0].length="+dbg_dmv_dsym[0].length);
}
dbg_dmv_dsym[i * dbg_titles_sym.length + j][dbg_index] = jta_mv [oj][dbg_titles_mv.length * indx + i]; //java.lang.ArrayIndexOutOfBoundsException: 3552
oj++;
}
}
}
dbgImgRemoveEmpty(dbg_xy);
dbgImgRemoveEmpty(dbg_xy);
dbgImgRemoveEmpty(dbg_dmv_dsym);
showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays();
sdfa_instance.showArrays(dbg_xy, dbg_owidth, dbg_oheight, true, "dbg_xy", dbg_titles_xy);
sdfa_instance.showArrays(dbg_mv, dbg_owidth, dbg_oheight, true, "dbg_mv", dbg_titles_mv);
sdfa_instance.showArrays(dbg_dmv_dsym, dbg_owidth, dbg_oheight, true, "dbg_dmv_dsym", dbg_dmv_dsym_titles);
}
if (debugLevel>-1) {
jtj.print(18, 6);
} }
Matrix y_minus_fx_weighted = new Matrix(y_minus_fx_a, y_minus_fx_a_weighted.length);
Matrix jtj = new Matrix(getJTJ(jta, weights)); // less operations than jt.times(jt.transpose());
Matrix jtj_inv = jtj.inverse(); Matrix jtj_inv = jtj.inverse();
Matrix jty = jt.times(y_minus_fx_weighted); Matrix jty = jt.times(y_minus_fx_weighted);
Matrix mrslt = jtj_inv.times(jty); Matrix mrslt = jtj_inv.times(jty);
double [] drslt = mrslt.getColumnPackedCopy(); double [] drslt = mrslt.getColumnPackedCopy();
GeometryCorrection.CorrVector rslt = geometryCorrection.getCorrVector(drslt); // wrong sign?
for (int i = 0; i < drslt.length; i++){
drslt[i] *= -1.0;
}
GeometryCorrection.CorrVector rslt = geometryCorrection.getCorrVector(drslt, par_mask);
if (debugLevel > -1){
System.out.println("solveCorr() rslt:");
System.out.println(rslt.toString());
}
return rslt; return rslt;
} }
......
src/main/java/EyesisCorrectionParameters.java
View file @ 0a050e16
...@@ -2049,9 +2049,12 @@ public class EyesisCorrectionParameters { ...@@ -2049,9 +2049,12 @@ public class EyesisCorrectionParameters {
public double ly_smpl_rms = 0.2; // 1; // Maximal RMS of the remaining tiles in a sample public double ly_smpl_rms = 0.2; // 1; // Maximal RMS of the remaining tiles in a sample
public double ly_disp_var = 0.5; // 2; // Maximal full disparity difference to 8 neighbors public double ly_disp_var = 0.5; // 2; // Maximal full disparity difference to 8 neighbors
public double ly_inf_frac = 0.5; // Relative weight of infinity calibration data public double ly_inf_frac = 0.5; // Relative weight of infinity calibration data
public boolean ly_on_scan = true; // Calculate and apply lazy eye correction after disparity scan public boolean ly_on_scan = true; // Calculate and apply lazy eye correction after disparity scan (poly or extrinsic)
public boolean ly_inf_en = true; // Simultaneously correct disparity at infinity public boolean ly_inf_en = true; // Simultaneously correct disparity at infinity (both poly and extrinsic)
public boolean ly_inf_force= false; // Force convergence correction during extrinsic, even with no infinity data
public boolean ly_poly = false; // Use polynomial correction, false - correct tilt/azimuth/roll of each sensor public boolean ly_poly = false; // Use polynomial correction, false - correct tilt/azimuth/roll of each sensor
// old fcorr parameters, reuse? // old fcorr parameters, reuse?
// public int fcorr_sample_size = 32; // Use square this size side to detect outliers // public int fcorr_sample_size = 32; // Use square this size side to detect outliers
...@@ -2675,6 +2678,7 @@ public class EyesisCorrectionParameters { ...@@ -2675,6 +2678,7 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"ly_inf_frac", this.ly_inf_frac +""); properties.setProperty(prefix+"ly_inf_frac", this.ly_inf_frac +"");
properties.setProperty(prefix+"ly_on_scan", this.ly_on_scan+""); properties.setProperty(prefix+"ly_on_scan", this.ly_on_scan+"");
properties.setProperty(prefix+"ly_inf_en", this.ly_inf_en+""); properties.setProperty(prefix+"ly_inf_en", this.ly_inf_en+"");
properties.setProperty(prefix+"ly_inf_force", this.ly_inf_force+"");
properties.setProperty(prefix+"ly_poly", this.ly_poly+""); properties.setProperty(prefix+"ly_poly", this.ly_poly+"");
properties.setProperty(prefix+"corr_magic_scale", this.corr_magic_scale +""); properties.setProperty(prefix+"corr_magic_scale", this.corr_magic_scale +"");
...@@ -3251,6 +3255,7 @@ public class EyesisCorrectionParameters { ...@@ -3251,6 +3255,7 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"ly_inf_frac")!=null) this.ly_inf_frac=Double.parseDouble(properties.getProperty(prefix+"ly_inf_frac")); if (properties.getProperty(prefix+"ly_inf_frac")!=null) this.ly_inf_frac=Double.parseDouble(properties.getProperty(prefix+"ly_inf_frac"));
if (properties.getProperty(prefix+"ly_on_scan")!=null) this.ly_on_scan=Boolean.parseBoolean(properties.getProperty(prefix+"ly_on_scan")); if (properties.getProperty(prefix+"ly_on_scan")!=null) this.ly_on_scan=Boolean.parseBoolean(properties.getProperty(prefix+"ly_on_scan"));
if (properties.getProperty(prefix+"ly_inf_en")!=null) this.ly_inf_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_en")); if (properties.getProperty(prefix+"ly_inf_en")!=null) this.ly_inf_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_en"));
if (properties.getProperty(prefix+"ly_inf_force")!=null) this.ly_inf_force=Boolean.parseBoolean(properties.getProperty(prefix+"ly_inf_force"));
if (properties.getProperty(prefix+"ly_poly")!=null) this.ly_poly=Boolean.parseBoolean(properties.getProperty(prefix+" ")); if (properties.getProperty(prefix+"ly_poly")!=null) this.ly_poly=Boolean.parseBoolean(properties.getProperty(prefix+" "));
if (properties.getProperty(prefix+"corr_magic_scale")!=null) this.corr_magic_scale=Double.parseDouble(properties.getProperty(prefix+"corr_magic_scale")); if (properties.getProperty(prefix+"corr_magic_scale")!=null) this.corr_magic_scale=Double.parseDouble(properties.getProperty(prefix+"corr_magic_scale"));
...@@ -3840,8 +3845,9 @@ public class EyesisCorrectionParameters { ...@@ -3840,8 +3845,9 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Maximal RMS of the remaining tiles in a sample", this.ly_smpl_rms, 3); gd.addNumericField("Maximal RMS of the remaining tiles in a sample", this.ly_smpl_rms, 3);
gd.addNumericField("Maximal full disparity difference to 8 neighbors", this.ly_disp_var, 3); gd.addNumericField("Maximal full disparity difference to 8 neighbors", this.ly_disp_var, 3);
gd.addNumericField("Relative weight of infinity calibration data", this.ly_inf_frac, 3); gd.addNumericField("Relative weight of infinity calibration data", this.ly_inf_frac, 3);
gd.addCheckbox ("Calculate and apply lazy eye correction after disparity scan (need to repeat)",this.ly_on_scan); gd.addCheckbox ("Calculate and apply lazy eye correction after disparity scan (poly or extrinsic), may repeat",this.ly_on_scan);
gd.addCheckbox ("Use infinity disparity (disable if there is not enough of infinity data)", this.ly_inf_en); gd.addCheckbox ("Use infinity disparity (disable if there is not enough of infinity data), both poly and extrinsic", this.ly_inf_en);
gd.addCheckbox ("Force convergence correction during extrinsic, even with no infinity data", this.ly_inf_force);
gd.addCheckbox ("*Use polynomial correction, false - correct tilt/azimuth/roll of each sensor)", this.ly_poly); gd.addCheckbox ("*Use polynomial correction, false - correct tilt/azimuth/roll of each sensor)", this.ly_poly);
gd.addMessage ("---"); gd.addMessage ("---");
// gd.addNumericField("Use square this size side to detect outliers", this.fcorr_sample_size, 0); // gd.addNumericField("Use square this size side to detect outliers", this.fcorr_sample_size, 0);
...@@ -4455,6 +4461,7 @@ public class EyesisCorrectionParameters { ...@@ -4455,6 +4461,7 @@ public class EyesisCorrectionParameters {
this.ly_inf_frac= gd.getNextNumber(); this.ly_inf_frac= gd.getNextNumber();
this.ly_on_scan= gd.getNextBoolean(); this.ly_on_scan= gd.getNextBoolean();
this.ly_inf_en= gd.getNextBoolean(); this.ly_inf_en= gd.getNextBoolean();
this.ly_inf_force= gd.getNextBoolean();
this.ly_poly= gd.getNextBoolean(); this.ly_poly= gd.getNextBoolean();
// this.fcorr_sample_size= (int)gd.getNextNumber(); // this.fcorr_sample_size= (int)gd.getNextNumber();
......
src/main/java/GeometryCorrection.java
View file @ 0a050e16
...@@ -94,6 +94,11 @@ public class GeometryCorrection { ...@@ -94,6 +94,11 @@ public class GeometryCorrection {
public CorrVector getCorrVector(double [] vector){ public CorrVector getCorrVector(double [] vector){
return new CorrVector(vector); return new CorrVector(vector);
} }
public CorrVector getCorrVector(
double [] vector,
boolean [] par_mask){
return new CorrVector(vector, par_mask);
}
public CorrVector getCorrVector(){ public CorrVector getCorrVector(){
return extrinsic_corr; return extrinsic_corr;
...@@ -118,6 +123,14 @@ public class GeometryCorrection { ...@@ -118,6 +123,14 @@ public class GeometryCorrection {
{ {
this.vector = new double[10]; this.vector = new double[10];
} }
public CorrVector (
double [] sym_vector,
boolean [] par_mask)
{
this.vector = toTarArray(sym_vector, par_mask);
}
public CorrVector ( public CorrVector (
double tilt0, double tilt1, double tilt2, double tilt0, double tilt1, double tilt2,
double azimuth0, double azimuth1, double azimuth2, double azimuth0, double azimuth1, double azimuth2,
...@@ -146,6 +159,8 @@ public class GeometryCorrection { ...@@ -146,6 +159,8 @@ public class GeometryCorrection {
return new CorrVector(vector); return new CorrVector(vector);
} }
public double [] toArray() public double [] toArray()
{ {
return vector; return vector;
...@@ -182,13 +197,22 @@ public class GeometryCorrection { ...@@ -182,13 +197,22 @@ public class GeometryCorrection {
public String toString() public String toString()
{ {
String s; String s;
double [] sym_vect = toSymArray(null);
double [] v = new double [vector.length]; double [] v = new double [vector.length];
double [] sv = new double [vector.length];
for (int i = 0; i < vector.length; i++){ for (int i = 0; i < vector.length; i++){
v[i] = vector[i]*180/Math.PI; v[i] = vector[i]*180/Math.PI;
sv[i] = sym_vect[i]*180/Math.PI;
} }
s = String.format("tilt (up): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) ); s = String.format("tilt (up): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) );
s += String.format("azimuth (right): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) ); s += String.format("azimuth (right): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) );
s += String.format("roll (CW): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[6], v[7], v[8], v[9] ); s += String.format("roll (CW): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[6], v[7], v[8], v[9] );
s += "Symmetrical vector first 5 as last 4 (rolls) are the same:\n";
s += " |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘|\n";
s += "0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖|\n";
s += String.format("0: %8.5f° 1:%8.5f° 2:%8.5f° 3:%8.5f° 4:%8.5f° 5:%8.5f°\n" , sv[0], sv[1], sv[2], sv[3], sv[4], sv[5]);
return s; return s;
} }
public void incrementVector(double [] incr) public void incrementVector(double [] incr)
...@@ -207,9 +231,183 @@ public class GeometryCorrection { ...@@ -207,9 +231,183 @@ public class GeometryCorrection {
public CorrVector clone(){ public CorrVector clone(){
return new CorrVector(this.vector.clone()); return new CorrVector(this.vector.clone());
} }
/**
* Get conversion matrix from symmetrical coordinates
* @return
*
* |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘|
* 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖|
*
*/
public double [][] dSym_j_dTar_i()
{
/* double [][] sym_to_tar = {
{-1.0, -1.0, 1.0, -1.0, -1.0, 1.0, 0.0, 0.0, 0.0, 0.0}, // t0
{-1.0, 1.0, -1.0, -1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0}, // t1
{ 1.0, -1.0, 1.0, 1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0}, // t2
{ 1.0, 1.0, 1.0, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0}, // a0
{-1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0}, // a1
{ 1.0, -1.0, -1.0, -1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0}, // a2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, // roll 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // roll 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // roll 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}};// roll 3
[[-2, -2, 2, -2, 0, 0],
[-2, 0, 0, -2, 2, -2],
[ 0, -2, 2, 0, 2, -2],
[ 2, 2, 2, -2, 0, 0],
[ 0, 2, 2, 0, 2, 2],
[ 2, 0, 0, -2, 2, 2]]
*/
//Previous does not consider movement of the 4-th sensor, so t3 = -t0+t1+t2), a3 = -(a0+a1+a2)
double [][] tar_to_sym = {
{-2.0, -2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // t0
{-2.0, 0.0, 0.0, -2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0}, // t1
{ 0.0, -2.0, 2.0, 0.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0}, // t2
{ 2.0, 2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // a0
{ 0.0, 2.0, 2.0, 0.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0}, // a1
{ 2.0, 0.0, 0.0, -2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0}, // a2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, // roll 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // roll 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // roll 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}};// roll 3
return tar_to_sym;
}
public double [][] dTar_j_dSym_i(){
// Matrix sym_to_tar = new Matrix(symToTARArray());
// Matrix tar_to_sym = sym_to_tar.inverse();
// return tar_to_sym.getArray();
/*
>>> a=[[-2, -2, 2, -2, 0, 0, 0, 0, 0, 0], [-2, 0, 0, -2, 2, -2, 0, 0, 0, 0], [0, -2, 2, 0, 2, -2, 0, 0, 0, 0], [2, 2, 2, -2, 0, 0, 0, 0, 0, 0], [0, 2, 2, 0, 2, 2, 0, 0, 0, 0],
... [2, 0, 0, -2, 2, 2, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1]]
>>> m = numpy.matrix(a)
>>> m
matrix([[-2, -2, 2, -2, 0, 0, 0, 0, 0, 0],
[-2, 0, 0, -2, 2, -2, 0, 0, 0, 0],
[ 0, -2, 2, 0, 2, -2, 0, 0, 0, 0],
[ 2, 2, 2, -2, 0, 0, 0, 0, 0, 0],
[ 0, 2, 2, 0, 2, 2, 0, 0, 0, 0],
[ 2, 0, 0, -2, 2, 2, 0, 0, 0, 0],
[ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]])
>>> numpy.linalg.inv(m)
matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, 0. , -0. , -0. , -0. ],
[-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ],
[ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. ]])
*/
double [][] sym_to_tar= {
// t0 t1 t2 a0 a1 a2 r0 r1 r2 r3
{-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym0
{-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym1
{ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym2
{-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym3
{-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym4
{ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym5
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 }, // sym6 = r0
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 }, // sym7 = r1
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 }, // sym8 = r2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 }}; // sym9 = r3
return sym_to_tar;
}
/**
* Get partial transposed Jacobian as 2d array (for one measurement set) from partial Jacobian for each sample
* with derivatives of port coordinates (all 4) by 3 tilts (ports 0..2), 3 azimuths (ports 0..2) and all 4 rolls
* Tilt and azimuth for port 3 is calculated so center would not move. Tilt is positive up, azimuth - right and
* roll - clockwise
*
* Result is transposed Jacobian (rows (9 or 10) - parameters, columns - port coordinate components (8). Parameters
* here are symmetrical, 0 is disparity-related (all to the center), remaining 9 preserve disparity and
* are only responsible for the "lazy eye" (last 4 are for roll):
*
* |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘|
* 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖|
*
* @param port_coord_deriv result of getPortsCoordinatesAndDerivatives: first index: port0x, port0y... port3y,
* second index - parameters [tilt0, tilt1, ..., roll3]
* @param par_mask array of 10 elements - which parameters to use (normally all true or all but first
* @return
*/
public double [][] getJtPartial(
double [][] port_coord_deriv,
boolean [] par_mask)
{
int num_pars = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
num_pars++;
}
double [][] sym_to_tar= dTar_j_dSym_i();
double [][] jt_part = new double[num_pars][port_coord_deriv.length];
int opar = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int i = 0; i < port_coord_deriv.length; i++){ // pxy index (0..7)
for (int k = 0; k < sym_to_tar[npar].length; k++){
jt_part[opar][i] += sym_to_tar[npar][k]* port_coord_deriv[i][k]; // transposing port_coord_deriv
}
}
opar++;
}
return jt_part;
}
// convert tilt0,... roll3 array to symmetrical coordinates [0] - to the center (disparity)
public double [] toSymArray(boolean [] par_mask)
{
return toSymArray(this.vector, par_mask);
}
public double [] toSymArray(
double [] tar_array,
boolean [] par_mask)
{
double [][] tar_to_sym = dSym_j_dTar_i();
int num_pars = 0;
for (int npar = 0; npar < vector.length; npar++) if ((par_mask==null) || par_mask[npar]) {
num_pars++;
}
double [] sym_array = new double [num_pars];
int opar = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int i = 0; i < tar_array.length; i++){
sym_array[opar] += tar_array[i] * tar_to_sym[i][npar];
}
opar++;
}
return sym_array;
}
public double [] toTarArray(
double [] sym_array,
boolean [] par_mask)
{
double [][] sym_to_tar = dTar_j_dSym_i();
double [] tar_array = new double [sym_to_tar[0].length];
for (int npar = 0; npar < LENGTH; npar++) {
int spar = 0;
for (int i = 0; i < LENGTH; i++) if ((par_mask==null) || par_mask[i]){
tar_array[npar] += sym_array[spar] * sym_to_tar[i][npar];
spar++;
}
}
return tar_array;
}
} }
public void setDistortion( public void setDistortion(
double focalLength, double focalLength,
double distortionC, double distortionC,
...@@ -741,22 +939,33 @@ public class GeometryCorrection { ...@@ -741,22 +939,33 @@ public class GeometryCorrection {
double py, double py,
double disparity) double disparity)
{ {
boolean new_ports = true; // false; // boolean new_ports = true; // false;
// if (new_ports) {
if (new_ports) { return getPortsCoordinatesAndDerivatives(
double [][][][] fj = getPortsCoordinatesAndDerivatives(
getCorrVector(), // CorrVector corr_vector, getCorrVector(), // CorrVector corr_vector,
false, // boolean calc_deriv, null, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
disparity); // double disparity) disparity); // double disparity)
return fj[0][0]; // } else {
} else { // return getPortsCoordinates_nocorr( // old, tested
return getPortsCoordinates_nocorr( // old, tested // px,
px, // py,
py, // disparity);
disparity); // }
} }
/**
* Convert pixel coordinates to relative (from -1.0 to +1.0 each) to apply polynomial corrections
* @param pXY pair of pixel X, pixel Y image coordinates
* @return pair of relative X, Y coordinates -n -1.0 ..+1.0 range
*/
public double [] getRelativeCoords(double [] pXY){
double [] relXY ={
2.0 * (pXY[0]/this.pixelCorrectionWidth - 0.5),
2.0 * (pXY[1]/this.pixelCorrectionWidth - 0.5),
};
return relXY;
} }
public double [][] getPortsCoordinates_nocorr( // old, tested public double [][] getPortsCoordinates_nocorr( // old, tested
...@@ -813,21 +1022,16 @@ public class GeometryCorrection { ...@@ -813,21 +1022,16 @@ public class GeometryCorrection {
* @return array of per port pairs of pixel shifts * @return array of per port pairs of pixel shifts
*/ */
public double [][][][] getPortsCoordinatesAndDerivatives( public double [][] getPortsCoordinatesAndDerivatives(
CorrVector corr_vector, CorrVector corr_vector,
boolean calc_deriv, double [][] pXYderiv, // if not null, should be double[8][]
double px, double px,
double py, double py,
double disparity) double disparity)
{ {
String dbg_s = corr_vector.toString();
double [][] pXY = new double [numSensors][2]; double [][] pXY = new double [numSensors][2];
double [][][] pXYderiv = calc_deriv? new double [numSensors][2][CorrVector.LENGTH] : null;
double [][][][] rslt = new double[calc_deriv? 2:1][][][];
rslt[0] = new double[1][][];
rslt[0][0] = pXY;
if (calc_deriv){
rslt[1] = pXYderiv;
}
double pXcd = px - 0.5 * this.pixelCorrectionWidth; double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight; double pYcd = py - 0.5 * this.pixelCorrectionHeight;
...@@ -864,16 +1068,19 @@ public class GeometryCorrection { ...@@ -864,16 +1068,19 @@ public class GeometryCorrection {
double s_roll = Math.sin(a_roll); double s_roll = Math.sin(a_roll);
pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0]; pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1]; pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
if (calc_deriv) { // double [][] pXYderiv = calc_deriv? new double [numSensors*2][CorrVector.LENGTH] : null;
if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
double dpXci_dazimuth = -corr_scale; double dpXci_dazimuth = -corr_scale;
double dpYci_dtilt = corr_scale; double dpYci_dtilt = corr_scale;
// double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale; double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale;
double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale; double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale;
double drD2rND_dri = 0.0; double drD2rND_dri = 0.0;
rri = 1.0; rri = 1.0;
for (int j = 0; j < a.length; j++){ for (int j = 0; j < a.length; j++){
drD2rND_dri += (j+1) * rri; drD2rND_dri += a[j] * (j+1) * rri;
rri *= ri; rri *= ri;
} }
...@@ -894,70 +1101,191 @@ public class GeometryCorrection { ...@@ -894,70 +1101,191 @@ public class GeometryCorrection {
double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt; double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt;
double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt; double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt;
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){ if (i < (numSensors - 1)){
pXYderiv[i][0][CorrVector.TILT_INDEX+i] = dxi_dtilt; pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dxi_dtilt;
pXYderiv[i][1][CorrVector.TILT_INDEX+i] = dyi_dtilt; pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dyi_dtilt;
pXYderiv[i][0][CorrVector.AZIMUTH_INDEX+i] = dxi_dazimuth; pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dxi_dazimuth;
pXYderiv[i][1][CorrVector.AZIMUTH_INDEX+i] = dyi_dazimuth; pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dyi_dazimuth;
} else { } else {
for (int j = 0; j < (numSensors - 1); j++){ for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[j][0][CorrVector.TILT_INDEX+i] = -dxi_dtilt; pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dxi_dtilt;
pXYderiv[j][1][CorrVector.TILT_INDEX+i] = -dyi_dtilt; pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dyi_dtilt;
pXYderiv[j][0][CorrVector.AZIMUTH_INDEX+i] = -dxi_dazimuth; pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dxi_dazimuth;
pXYderiv[j][1][CorrVector.AZIMUTH_INDEX+i] = -dyi_dazimuth; pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dyi_dazimuth;
} }
} }
double dxi_droll = -s_roll * pXid + c_roll * pYid; double dxi_droll = -s_roll * pXid + c_roll * pYid;
double dyi_droll = -c_roll * pXid - s_roll * pYid; double dyi_droll = -c_roll * pXid - s_roll * pYid;
pXYderiv[i][0][CorrVector.ROLL_INDEX+i] = dxi_droll; pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dxi_droll;
pXYderiv[i][1][CorrVector.ROLL_INDEX+i] = dyi_droll; pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dyi_droll;
} }
} }
return rslt; return pXY;
} }
public double [][] getPortsCoordinatesAndDerivatives(
double [] dbg_a_vector, // replace actual radial distortion coefficients
CorrVector corr_vector,
double [][] pXYderiv, // if not null, should be double[8][]
double px,
double py,
double disparity)
{
// String dbg_s = corr_vector.toString();
double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
if (dbg_a_vector != null){
a = dbg_a_vector;
}
double corr_scale = focalLength/(0.001*pixelSize);
double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * this.rXY[i][1];
// add misalignment, for small angles express in non-distorted pixels
double pXci = pXci0 - corr_vector.getAzimuth(i) * corr_scale;
double pYci = pYci0 + corr_vector.getTilt(i) * corr_scale;
// calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
double ri = rNDi* ri_scale; // relative to distortion radius
// double rD2rND = (1.0 - distortionA8 - distortionA7 - distortionA6 - distortionA5 - distortionA - distortionB - distortionC);
double rD2rND = 1.0;
double rri = 1.0;
for (int j = 0; j < a.length; j++){
rri *= ri;
rD2rND += a[j]*(rri - 1.0); // Fixed
}
double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND;
// individual rotate (check sign)
double a_roll = (this.roll[i] - this.common_roll) * Math.PI/180.0 + corr_vector.getRoll(i) ;
double c_roll = Math.cos(a_roll);
double s_roll = Math.sin(a_roll);
pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
// double [][] pXYderiv = calc_deriv? new double [numSensors*2][CorrVector.LENGTH] : null;
if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
double dpXci_dazimuth = -corr_scale;
double dpYci_dtilt = corr_scale;
double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale;
double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale;
double drD2rND_dri = 0.0;
rri = 1.0;
for (int j = 0; j < a.length; j++){
drD2rND_dri += a[j] * (j+1) * rri;
rri *= ri;
}
double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
// TODO: understand! hack - testing seems to work, but why?
drD2rND_dazimuth /= rD2rND;
drD2rND_dtilt /= rD2rND;
// double pXid = pXci * rD2rND;
// double pYid = pYci * rD2rND;
double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXid * drD2rND_dazimuth;
double dpYid_dazimuth = pYid * drD2rND_dazimuth;
double dpXid_dtilt = pXid * drD2rND_dtilt;
double dpYid_dtilt = dpYci_dtilt * rD2rND + pYid * drD2rND_dtilt;
// pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
// pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
double dxi_dazimuth = c_roll * dpXid_dazimuth + s_roll * dpYid_dazimuth;
double dyi_dazimuth = -s_roll * dpXid_dazimuth + c_roll * dpYid_dazimuth;
double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt;
double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt;
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dxi_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dyi_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dxi_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dyi_dazimuth;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dxi_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dyi_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dxi_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dyi_dazimuth;
}
}
double dxi_droll = -s_roll * pXid + c_roll * pYid;
double dyi_droll = -c_roll * pXid - s_roll * pYid;
pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dxi_droll;
pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dyi_droll;
}
}
return pXY;
}
// debug version, calculates derivatives as differences // debug version, calculates derivatives as differences
public double [][][][] getPortsCoordinatesAndDerivatives( public double [][] getPortsCoordinatesAndDerivatives(
double [] dbg_a_vector, // replace actual radial distortion coefficients
double delta, // 1e-6 double delta, // 1e-6
CorrVector corr_vector, CorrVector corr_vector,
double [][] pXYderiv, // if not null, should be double[8][]
double px, double px,
double py, double py,
double disparity) double disparity)
{ {
double [][][][] rslt = new double[2][][][];
rslt[0] = getPortsCoordinatesAndDerivatives( double [][] pXYderiv0 = new double[8][];
double [][] rslt = getPortsCoordinatesAndDerivatives(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_vector, // CorrVector corr_vector, corr_vector, // CorrVector corr_vector,
false, // boolean calc_deriv, pXYderiv0, // null, // false, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
disparity // double disparity disparity // double disparity
)[0]; );
double [][][] pXYderiv = new double [numSensors][2][CorrVector.LENGTH]; for (int i = 0; i < pXYderiv.length; i++){
rslt[1] = pXYderiv; pXYderiv[i] = new double [CorrVector.LENGTH];
}
for (int nPar = 0; nPar < CorrVector.LENGTH; nPar++){ for (int nPar = 0; nPar < CorrVector.LENGTH; nPar++){
CorrVector cv_delta_p = corr_vector.clone(); CorrVector cv_delta_p = corr_vector.clone();
CorrVector cv_delta_m = corr_vector.clone(); CorrVector cv_delta_m = corr_vector.clone();
cv_delta_p.vector[nPar] += 0.5 *delta; cv_delta_p.vector[nPar] += 0.5 *delta;
cv_delta_m.vector[nPar] -= 0.5 *delta; cv_delta_m.vector[nPar] -= 0.5 *delta;
double [][] rslt_p = getPortsCoordinatesAndDerivatives( double [][] rslt_p = getPortsCoordinatesAndDerivatives(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
cv_delta_p, // CorrVector corr_vector, cv_delta_p, // CorrVector corr_vector,
false, // boolean calc_deriv, null, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
disparity // double disparity disparity // double disparity
)[0][0]; );
double [][] rslt_m = getPortsCoordinatesAndDerivatives( double [][] rslt_m = getPortsCoordinatesAndDerivatives(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
cv_delta_m, // CorrVector corr_vector, cv_delta_m, // CorrVector corr_vector,
false, // boolean calc_deriv, null, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
disparity // double disparity disparity // double disparity
)[0][0]; );
for (int sens = 0; sens < numSensors; sens++){ for (int sens = 0; sens < numSensors; sens++){
for (int dir = 0; dir < 2; dir++){ for (int dir = 0; dir < 2; dir++){
pXYderiv[sens][dir][nPar] = (rslt_p[sens][dir] - rslt_m[sens][dir]) / delta; pXYderiv[2 * sens +dir][nPar] = (rslt_p[sens][dir] - rslt_m[sens][dir]) / delta;
} }
} }
} }
......
src/main/java/ImageDtt.java
View file @ 0a050e16
...@@ -1220,8 +1220,10 @@ public class ImageDtt { ...@@ -1220,8 +1220,10 @@ public class ImageDtt {
centersXY[ip][0] -= shiftXY[ip][0]; centersXY[ip][0] -= shiftXY[ip][0];
centersXY[ip][1] -= shiftXY[ip][1]; centersXY[ip][1] -= shiftXY[ip][1];
} }
// TODO: use correction after disparity applied (to work for large disparity values)
if (fine_corr != null){ if (fine_corr != null){
// old correction
/*
double tX = (2.0 * tileX)/tilesX - 1.0; // -1.0 to +1.0 double tX = (2.0 * tileX)/tilesX - 1.0; // -1.0 to +1.0
double tY = (2.0 * tileY)/tilesY - 1.0; // -1.0 to +1.0 double tY = (2.0 * tileY)/tilesY - 1.0; // -1.0 to +1.0
for (int ip = 0; ip < centersXY.length; ip++){ for (int ip = 0; ip < centersXY.length; ip++){
...@@ -1235,6 +1237,20 @@ public class ImageDtt { ...@@ -1235,6 +1237,20 @@ public class ImageDtt {
fine_corr[ip][d][4]*tY+ fine_corr[ip][d][4]*tY+
fine_corr[ip][d][5]); fine_corr[ip][d][5]);
} }
*/
for (int ip = 0; ip < centersXY.length; ip++){
double [] tXY = geometryCorrection.getRelativeCoords(centersXY[ip]);
for (int d = 0; d <2; d++) {
centersXY[ip][d] -= (
fine_corr[ip][d][0]*tXY[0]*tXY[0]+
fine_corr[ip][d][1]*tXY[1]*tXY[1]+
fine_corr[ip][d][2]*tXY[0]*tXY[1]+
fine_corr[ip][d][3]*tXY[0]+
fine_corr[ip][d][4]*tXY[1]+
fine_corr[ip][d][5]);
}
}
} }
} }
if ((globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY) && (chn == 2)) { if ((globalDebugLevel > -1) && (tileX == debug_tileX) && (tileY == debug_tileY) && (chn == 2)) {
......
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