Skip to content

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

tested new extrinsic adjustment including ERS rotation

parent cb607a02
Hide whitespace changes
Inline Side-by-side
Showing with 1697 additions and 126 deletions
src/main/java/com/elphel/imagej/cameras/CLTParameters.java
View file @ b47dba20
......@@ -177,10 +177,14 @@ public class CLTParameters {
public boolean ly_inf_en = true; // Simultaneously correct disparity at infinity (both poly and extrinsic)
public boolean ly_aztilt_en = true; // Adjust azimuths and tilts
public boolean ly_diff_roll_en = true; // Adjust differential rolls (3 of 4 angles)
public boolean ly_focalLength= true; // Correct scales (focal length temperature? variations)
public boolean ly_focalLength= true; // Correct scales (focal length temperature? variations)
public boolean ly_com_roll= false; // Enable common roll (valid for high disparity range only)
public boolean ly_ers_rot= true; // Enable ERS correction of the camera rotation
public boolean ly_ers_lin= false; // Enable ERS correction of the camera linear movement
public int ly_par_sel = 0; // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use checkbox selections above)
public int ly_debug_level = 0; // LY debug level
public boolean ly_right_left= false; // equalize weights of right/left FoV (use with horizon in both halves and gross infinity correction)
public int ly_per_quad = 10; // minimal tiles per quadrant (not counting the worst) tp proceed
......@@ -966,8 +970,12 @@ public class CLTParameters {
properties.setProperty(prefix+"ly_diff_roll_en", this.ly_diff_roll_en+"");
properties.setProperty(prefix+"ly_focalLength", this.ly_focalLength+"");
properties.setProperty(prefix+"ly_com_roll", this.ly_com_roll+"");
properties.setProperty(prefix+"ly_ers_rot", this.ly_ers_rot+"");
properties.setProperty(prefix+"ly_ers_lin", this.ly_ers_lin+"");
properties.setProperty(prefix+"ly_par_sel", this.ly_par_sel+"");
properties.setProperty(prefix+"ly_debug_level", this.ly_debug_level+"");
properties.setProperty(prefix+"ly_right_left", this.ly_right_left+"");
properties.setProperty(prefix+"ly_per_quad", this.ly_per_quad +"");
......@@ -1683,8 +1691,11 @@ public class CLTParameters {
if (properties.getProperty(prefix+"ly_diff_roll_en")!=null) this.ly_diff_roll_en=Boolean.parseBoolean(properties.getProperty(prefix+"ly_diff_roll_en"));
if (properties.getProperty(prefix+"ly_focalLength")!=null) this.ly_focalLength=Boolean.parseBoolean(properties.getProperty(prefix+"ly_focalLength"));
if (properties.getProperty(prefix+"ly_com_roll")!=null) this.ly_com_roll=Boolean.parseBoolean(properties.getProperty(prefix+"ly_com_roll"));
if (properties.getProperty(prefix+"ly_ers_rot")!=null) this.ly_ers_rot=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_rot"));
if (properties.getProperty(prefix+"ly_ers_lin")!=null) this.ly_ers_lin=Boolean.parseBoolean(properties.getProperty(prefix+"ly_ers_lin"));
if (properties.getProperty(prefix+"ly_par_sel")!=null) this.ly_par_sel=Integer.parseInt(properties.getProperty(prefix+"ly_par_sel"));
if (properties.getProperty(prefix+"ly_debug_level")!=null) this.ly_debug_level=Integer.parseInt(properties.getProperty(prefix+"ly_debug_level"));
if (properties.getProperty(prefix+"ly_right_left")!=null) this.ly_right_left=Boolean.parseBoolean(properties.getProperty(prefix+"ly_right_left"));
if (properties.getProperty(prefix+"ly_per_quad")!=null) this.ly_per_quad=Integer.parseInt(properties.getProperty(prefix+"ly_per_quad"));
......@@ -2436,7 +2447,7 @@ public class CLTParameters {
gd.addTab ("Lazy eye", "Lazy eye parameters");
gd.addMessage ("--- main-to-aux depth map parameters ---");
gd.addNumericField("Minimal reference (main) channel orrelation strength", this.ly_gt_strength, 3);
gd.addNumericField("Minimal reference (main) channel correlation strength", this.ly_gt_strength, 3);
gd.addCheckbox ("Use window for AUX tiles to reduce weight of the hi-res tiles near low-res tile boundaries", this.ly_gt_use_wnd);
gd.addNumericField("Aux disparity thershold to split FG and BG (and disable AUX tile for adjustment)", this.ly_gt_rms, 3);
gd.addMessage ("--- others ---");
......@@ -2446,9 +2457,15 @@ public class CLTParameters {
gd.addCheckbox ("Adjust differential rolls", this.ly_diff_roll_en,"Adjust differential rolls (3 of 4 rolls, keeping average roll)");
gd.addCheckbox ("Correct scales (focal length temperature? variations)", this.ly_focalLength);
gd.addCheckbox ("Enable common roll adjustment (valid for high disparity range scans only)", this.ly_com_roll);
gd.addCheckbox ("Enable ERS correction of the camera rotation", this.ly_ers_rot);
gd.addCheckbox ("Enable ERS correction of the camera linear movement", this.ly_ers_lin);
gd.addNumericField("Manual parameter mask selection (0 use checkboxes above)", this.ly_par_sel, 0, 5,"",
"bit 0 - sym0, bit1 - sym1, ...");
gd.addNumericField("Debug level for lazy eye/ers processing", this.ly_debug_level, 0, 5,"",
"Active when global debug level > -1");
gd.addCheckbox ("Equalize weights of right/left FoV", this.ly_right_left,
"Use this mode use with horizon visible in both FoV halves when gross infinity correction is needed");
......@@ -3273,7 +3290,10 @@ public class CLTParameters {
this.ly_diff_roll_en= gd.getNextBoolean();
this.ly_focalLength= gd.getNextBoolean();
this.ly_com_roll= gd.getNextBoolean();
this.ly_ers_rot= gd.getNextBoolean();
this.ly_ers_lin= gd.getNextBoolean();
this.ly_par_sel= (int) gd.getNextNumber();
this.ly_debug_level= (int) gd.getNextNumber();
this.ly_right_left= gd.getNextBoolean();
......
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ b47dba20
......@@ -709,7 +709,9 @@ private Panel panel1,
addButton("LWIR_TEST", panelClt_GPU, color_conf_process);
addButton("LWIR_ACQUIRE", panelClt_GPU, color_conf_process);
addButton("IMU main", panelClt_GPU, color_conf_process);
addButton("ERS main", panelClt_GPU, color_process);
addButton("IMU aux", panelClt_GPU, color_conf_process_aux);
addButton("ERS aux", panelClt_GPU, color_process_aux);
plugInFrame.add(panelClt_GPU);
}
......@@ -4318,7 +4320,8 @@ private Panel panel1,
} else if (label.equals("CLT 4 images") ||
label.equals("CLT apply fine corr") ||
label.equals("CLT infinity corr") ||
label.equals("CORR TEST" )) {
label.equals("CORR TEST" ) ||
label.equals("ERS main")) {
boolean apply_corr = label.equals("CLT apply fine corr");
boolean infinity_corr = label.equals("CLT infinity corr");
DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
......@@ -4377,7 +4380,19 @@ private Panel panel1,
int num_infinity_corr = infinity_corr? CLT_PARAMETERS.inf_repeat : 1;
if ( num_infinity_corr < 1) num_infinity_corr = 1;
for (int i_infinity_corr = 0; i_infinity_corr < num_infinity_corr; i_infinity_corr++) {
if (label.equals("CORR TEST")) {
if (label.equals("ERS main")) {
QUAD_CLT.processCLTQuadCorrsTestERS(
CLT_PARAMETERS, // EyesisCorrectionParameters.DCTParameters dct_parameters,
DEBAYER_PARAMETERS, //EyesisCorrectionParameters.DebayerParameters debayerParameters,
COLOR_PROC_PARAMETERS, //EyesisCorrectionParameters.ColorProcParameters colorProcParameters,
CHANNEL_GAINS_PARAMETERS, //CorrectionColorProc.ColorGainsParameters channelGainParameters,
RGB_PARAMETERS, //EyesisCorrectionParameters.RGBParameters rgbParameters,
apply_corr,
infinity_corr, // calculate and apply geometry correction at infinity
THREADS_MAX, //final int threadsMax, // maximal number of threads to launch
UPDATE_STATUS, //final boolean updateStatus,
DEBUG_LEVEL); //final int debugLevel);
} else if (label.equals("CORR TEST")) {
QUAD_CLT.processCLTQuadCorrsTest(
CLT_PARAMETERS, // EyesisCorrectionParameters.DCTParameters dct_parameters,
DEBAYER_PARAMETERS, //EyesisCorrectionParameters.DebayerParameters debayerParameters,
......
src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java
View file @ b47dba20
......@@ -100,6 +100,7 @@ public class Corr2dLMA {
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization terms
private double pure_weight; // weight of samples only
private double [] values;
// next values are only updated after success
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
......@@ -763,7 +764,7 @@ public class Corr2dLMA {
return dbg_img;
}
private double [] getFxJt( // USED in lwir
private double [] getFxJt(
double [] vector,
double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
if (this.gaussian_mode) return getFxJt_gaussian(vector, jt);
......@@ -1318,7 +1319,7 @@ public class Corr2dLMA {
////////////////////////////////////////////////////////////////////////
private boolean debugJt( // not used in lwir
private boolean debugJt(
double delta,
double [] vector) {
int num_points = this.values.length;
......@@ -1653,7 +1654,7 @@ public class Corr2dLMA {
return wjtymfx;
}
public boolean runLma( // USED in lwir
public boolean runLma(
double lambda, // 0.1
double lambda_scale_good,// 0.5
double lambda_scale_bad, // 8.0
......@@ -1713,7 +1714,7 @@ public class Corr2dLMA {
// returns {success, done}
public boolean [] lmaStep( // USED in lwir
public boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
......@@ -1821,5 +1822,4 @@ public class Corr2dLMA {
}
return rslt;
}
}
src/main/java/com/elphel/imagej/tileprocessor/ExtrinsicAdjustment.java 0 → 100644
View file @ b47dba20
package com.elphel.imagej.tileprocessor;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import Jama.Matrix;
/**
**
** ExtrinsicAdjustment - Adjust cameras extrinsic parameters and ERS correction
**
** Copyright (C) 2019 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** ExtrinsicAdjustment.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
public class ExtrinsicAdjustment {
static final int NUM_SENSORS = 4;
static final int INDX_DISP = 0; // composite
static final int INDX_STRENGTH = 1;
static final int INDX_X0 = 2;
static final int INDX_TARGET = 10; // target disparity
static final int INDX_DIFF = 11; // differential disparity (now w/o magic composite =target + diff)
static final int INDX_DYDDISP0 =12; // derivative of pixel y over disparity (for ERS)
static final int INDX_PX = 16;
static final int INDX_DD0 = 18;
static final int INDX_ND0 = 22;
static final int INDX_LENGTH = 26;
static final int POINTS_SAMPLE = 2 * NUM_SENSORS +1; // points per sample residual disparity, 4*dd, *nd
static final String [] DATA_TITLES = {
"Disparity", "Strength",
"DX-0","DY-0","DX-1","DY-1","DX-2","DY-2","DX-3","DY-3",
"Target Disparity","Diff. Disparity",
"dY_dD-0","dY_dD-1","dY_dD-2","dY_dD-3",
"pX","pY",
"DD-0", "DD-1","DD-2","DD-3","ND-0", "ND-1","ND-2","ND-3"};
// next values are only updated after success
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization terms
private boolean [] force_disparity = null; // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
private double pure_weight; // weight of samples only
private double [] values;
private GeometryCorrection.CorrVector corr_vector = null;
private boolean [] par_mask = null;
private boolean use_rig_offsets = false;
private double [][] measured_dsxy = null;
private double [][] dy_ddisparity = null; // conveniently extracted from dsdn
private double [][] x0y0 = null; //
public GeometryCorrection geometryCorrection = null;
public int clusterSize;
public int clustersX;
public int clustersY;
public ExtrinsicAdjustment (
GeometryCorrection gc,
int clusterSize,
int clustersX,
int clustersY) {
geometryCorrection = gc;
this.clusterSize = clusterSize;
this.clustersX = clustersX;
this.clustersY = clustersY;
}
private void showInput(double[][] data, String title) {
int clusters = clustersX * clustersY;
double [][] pixels = new double [ExtrinsicAdjustment.INDX_LENGTH][clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
if (data[cluster] != null) {
for (int c = 0; c < data[cluster].length; c++) {
pixels[c][cluster] = data[cluster][c];
}
} else {
for (int c = 0; c < pixels.length; c++) {
pixels[c][cluster] = (c == ExtrinsicAdjustment.INDX_STRENGTH)? 0.0: Double.NaN;
}
}
}
(new ShowDoubleFloatArrays()).showArrays(
pixels,
clustersX,
clustersY,
true,
title,
ExtrinsicAdjustment.DATA_TITLES);
}
private void showX0Y0(double [][] xy0, String title) {
String [] titles = {"xnd-0","ynd-0","xnd-1","ynd-1","xnd-2","ynd-2","xnd-3","ynd-3"};
int clusters = clustersX * clustersY;
double [][] pixels = new double [titles.length][clusters];
for (int cluster = 0; cluster < clusters; cluster++) {
if (xy0[cluster] != null) {
for (int c = 0; c < xy0[cluster].length; c++) {
pixels[c][cluster] = xy0[cluster][c];
}
} else {
for (int c = 0; c < pixels.length; c++) {
pixels[c][cluster] = Double.NaN;
}
}
}
(new ShowDoubleFloatArrays()).showArrays(
pixels,
clustersX,
clustersY,
true,
title,
titles);
}
public GeometryCorrection.CorrVector solveCorr (
boolean use_disparity, // adjust disparity-related extrinsics
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
// data, using just radial distortions
boolean common_roll, // Enable common roll (valid for high disparity range only)
boolean corr_focalLength, // Correct scales (focal length temperature? variations)
boolean ers_rot, // Enable ERS correction of the camera rotation
boolean ers_lin, // Enable ERS correction of the camera linear movement
// add balancing-related here?
int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
double weight_disparity,
double weight_lazyeye,
double [][] measured_dsxy_in, //
boolean [] force_disparity_in, // boolean [] force_disparity,
GeometryCorrection geometryCorrection,
boolean use_main, // corr_rots_aux != null;
GeometryCorrection.CorrVector corr_vector_meas,
double [] old_new_rms, // should be double[2]
int debugLevel)
{
this.corr_vector = corr_vector_meas.clone();
this.use_rig_offsets = false;
this.measured_dsxy = measured_dsxy_in;
this.force_disparity = force_disparity_in;
final Matrix [] corr_rots_aux = null;
Matrix [][] deriv_rots_aux = null;
final Matrix [] corr_rots = use_main ? corr_rots_aux : corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
final Matrix [][] deriv_rots = use_main ? deriv_rots_aux : corr_vector.getRotDeriveMatrices();
boolean dbg_images = debugLevel > 0; // 1;
int clusters =clustersX * clustersY;
// dy_ddisparity = new double[clusters][];
// x0y0 = new double[clusters][];
if (dbg_images) {
showInput(
measured_dsxy, // double[][] data,
"input data");// String title);
}
x0y0 = getXYNondistorted(
corr_vector,
true); // boolean set_dydisp)
if (dbg_images) {
showX0Y0(
x0y0, // double[][] data,
"nondistorted X0Y0");// String title);
}
this.par_mask = geometryCorrection.getParMask(
use_disparity, // has_disparity, // boolean use_disparity,
use_aztilts, // Adjust azimuths and tilts excluding disparity
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,// boolean common_roll,
corr_focalLength, // boolean corr_focalLength);
ers_rot, // boolean ers_rot, // Enable ERS correction of the camera rotation
ers_lin, // boolean ers_lin, // Enable ERS correction of the camera linear movement
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
this.weights = getWeights(
measured_dsxy, // double [][] measured_dsxy,
// force_disparity, // boolean [] force_disparity, // same dimension as dsdn, true if disparity should be controlled
weight_disparity, // double weight_disparity,
weight_lazyeye); // double weight_lazyeye);
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 lma_OK = runLma(
lambda, // double lambda, // 0.1
lambda_scale_good, // double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
debugLevel); // int debug_level)
return lma_OK? corr_vector : null;
}
private double [][] getXYNondistorted(
GeometryCorrection.CorrVector corr_vector,
boolean set_dydisp){
int clusters =clustersX * clustersY;
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i)
double [][] xyND = new double[clusters][];
if (set_dydisp) {
dy_ddisparity = new double[clusters][];
} for (int cluster = 0; cluster < clusters; cluster++) {
if (measured_dsxy[cluster] != null) {
if (set_dydisp) {
dy_ddisparity[cluster] = new double[NUM_SENSORS];
for (int i = 0; i < NUM_SENSORS; i++) {
dy_ddisparity[cluster][i] = measured_dsxy[cluster][INDX_DYDDISP0 + i];
}
}
xyND[cluster] = geometryCorrection.getPortsNonDistortedCoordinatesAndDerivatives( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] pXYNDderiv, // if not null, should be double[8][]
dy_ddisparity[cluster], // dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu, // double [] imu,
measured_dsxy[cluster][INDX_PX + 0], // double px,
measured_dsxy[cluster][INDX_PX + 1], // double py,
measured_dsxy[cluster][INDX_TARGET]); // double disparity);
}
}
return xyND;
}
private double [] getYminusFx(
GeometryCorrection.CorrVector corr_vector)
{
int clusters = clustersX * clustersY;
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE];
double [] imu = corr_vector.getIMU(); // i)
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivatives( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] DDNDderiv, // if not null, should be double[8][]
dy_ddisparity[cluster], // double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu, // double [] imu,
x0y0[cluster], // double [] pXYND0, // per-port non-distorted coordinates corresponding to the correlation measurements
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
//arraycopy(Object src, int srcPos, Object dest, int destPos, int length)
// System.arraycopy(src_pixels, 0, dst_pixels, 0, src_pixels.length); /* for the borders closer to 1/2 kernel size*/
ddnd[0] = -ddnd[0];
if ((force_disparity != null) && force_disparity[cluster]) {
ddnd[0] -= measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
}
/// ddnd[0] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] - ddnd[0];
for (int i = 0; i < NUM_SENSORS; i++) {
ddnd[i + 1] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + i] - ddnd[i + 1];
ddnd[i + 5] = -measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + i] - ddnd[i + 5];
}
System.arraycopy(ddnd, 0, y_minus_fx, cluster * POINTS_SAMPLE, POINTS_SAMPLE);
}
return y_minus_fx;
}
private double [] getWYmFxRms( // USED in lwir
double [] fx) {
int clusters = clustersX * clustersY;
double rms = 0, rms_pure = 0;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
int indx0 = POINTS_SAMPLE * cluster + 0;
// force_disparity - compensate measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF],
// false - keep (so force fx==0
double d = - fx[indx0];
if ((force_disparity != null) && force_disparity[cluster]) {
d -= measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
}
fx[indx0] = this.weights[indx0] * d;
rms += fx[indx0]*d; // sum of weights
for (int cam = 0; cam < NUM_SENSORS; cam++) {
int indx = indx0 + cam + 1;
d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights
indx = indx0 + cam + 5; // nd
d = (-measured_dsxy[cluster][ExtrinsicAdjustment.INDX_ND0 + cam] - fx[indx]);
fx[indx] = this.weights[indx] * d;
rms += fx[indx] * d; // sum of weights
}
}
rms_pure = Math.sqrt(rms)/this.pure_weight;
// Calculate other regularization terms here if needed
rms = Math.sqrt(rms);
double [] rslt = {rms, rms_pure};
return rslt;
}
private double [] getWeights(
double [][] measured_dsxy,
double weight_disparity,
double weight_lazyeye)
{
int clusters = clustersX * clustersY;
double [] weights = new double [clusters * POINTS_SAMPLE];
double sw = 0.0;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double w;
// if ((force_disparity != null) && force_disparity[cluster]) {
w = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH] * weight_disparity;
weights[cluster * POINTS_SAMPLE + 0] = w;
sw += w;
// }
w = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_STRENGTH] * weight_lazyeye;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = w;
}
sw += w * (POINTS_SAMPLE - 1);
}
if (sw <= 0.0) {
return null;
}
double k = 1.0/sw;
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
weights[cluster * POINTS_SAMPLE + 0] *= k;
double w = weights[cluster * POINTS_SAMPLE + 1] * k;
for (int i = 1; i < POINTS_SAMPLE; i++) {
weights[cluster * POINTS_SAMPLE + i] = w;
}
}
this.pure_weight = 1.0;
return weights;
}
private double [] getFx(
GeometryCorrection.CorrVector corr_vector)
{
int clusters = clustersX * clustersY;
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i)
double [] y_minus_fx = new double [clusters * POINTS_SAMPLE];
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
double [] ddnd = geometryCorrection.getPortsDDNDAndDerivatives( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
null, // double [][] DDNDderiv, // if not null, should be double[8][]
dy_ddisparity[cluster], // double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu, // double [] imu,
x0y0[cluster], // double [] pXYND0, // per-port non-distorted coordinates corresponding to the correlation measurements
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
//arraycopy(Object src, int srcPos, Object dest, int destPos, int length)
// System.arraycopy(src_pixels, 0, dst_pixels, 0, src_pixels.length); /* for the borders closer to 1/2 kernel size*/
ddnd[0] = ddnd[0];
for (int i = 0; i < NUM_SENSORS; i++) {
ddnd[i + 1] = ddnd[i + 1];
ddnd[i + 5] = ddnd[i + 5];
}
System.arraycopy(ddnd, 0, y_minus_fx, cluster*POINTS_SAMPLE, POINTS_SAMPLE);
}
return y_minus_fx;
}
private double [][] getJacobianTransposed(
GeometryCorrection.CorrVector corr_vector,
double delta){
int clusters = clustersX * clustersY;
int num_pars = getNumPars();
double [][] jt = new double [num_pars][clusters * POINTS_SAMPLE ];
double rdelta = 1.0/delta;
for (int par = 0; par < num_pars; par++) {
double [] pars = new double[num_pars];
pars[par] = delta;
GeometryCorrection.CorrVector corr_delta = geometryCorrection.getCorrVector(pars, par_mask);
GeometryCorrection.CorrVector corr_vectorp = corr_vector.clone();
GeometryCorrection.CorrVector corr_vectorm = corr_vector.clone();
corr_vectorp.incrementVector(corr_delta, 0.5);
corr_vectorm.incrementVector(corr_delta, -0.5);
double [] fx_p = getFx(corr_vectorp);
double [] fx_m = getFx(corr_vectorm);
for (int i = 0; i < fx_p.length; i++) {
jt[par][i] = (fx_p[i] - fx_m[i])*rdelta;
}
}
return jt;
}
private String [] getSymNames() {
int num_pars = getNumPars();
String [] names = new String[num_pars];
int indx = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]){
names[indx++] = "S"+i;
}
return names;
}
private double dbgJacobians(
GeometryCorrection.CorrVector corr_vector,
double delta,
boolean graphic) {
int gap = 10; //pix
int clusters = clustersX * clustersY;
int num_pars = getNumPars();
String [] titles = getSymNames();
double [][] jt = getJacobianTransposed(corr_vector);
double [][] jt_delta = getJacobianTransposed(corr_vector, delta);
double tot_error = 0;
double [][] err = new double [num_pars][POINTS_SAMPLE];
double [] err_par = new double [num_pars];
for (int par = 0; par < num_pars; par++) {
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
for (int i = 0; i < POINTS_SAMPLE; i++) {
int indx = cluster * POINTS_SAMPLE+ i;
if (Math.abs(jt[par][indx] - jt_delta[par][indx]) > err[par][i]) {
err[par][i] = Math.abs(jt[par][indx] - jt_delta[par][indx]);
}
}
}
for (int i = 0; i < POINTS_SAMPLE; i++) {
if (err[par][i] > err_par[par]) {
err_par[par] = err[par][i];
}
}
if (err_par[par] > tot_error) {
tot_error = err_par[par];
}
}
System.out.println("Maximal error for all parameters = "+tot_error);
System.out.println(String.format("%4s %10.6s %10s %10s %10s %10s %10s %10s %10s %10s %10s",
"","max","disp", "dd0", "dd1", "dd2", "dd3", "nd0", "nd1", "nd2", "nd3"));
for (int par = 0; par < num_pars; par++) {
System.out.println(String.format("%4s %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f",
titles[par],err_par[par],err[par][0],
err[par][1],err[par][2],err[par][3],err[par][4],err[par][5],err[par][6],err[par][7],err[par][8]));
}
if (graphic) {
String [] titles3 = new String[num_pars * 3];
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
double [][] dbg_img = new double [num_pars * 3][width*height];
for (int par = 0; par < num_pars; par++) {
titles3[3 * par + 0] = titles[par]+"";
titles3[3 * par + 1] = titles[par]+"_delta";
titles3[3 * par + 2] = titles[par]+"_diff";
for (int mode = 0; mode < POINTS_SAMPLE; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX);
int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode;
if (measured_dsxy[cluster] != null){
dbg_img[3 * par + 0][pix] = jt[par][indx];
dbg_img[3 * par + 1][pix] = jt_delta[par][indx];
dbg_img[3 * par + 2][pix] = jt[par][indx] - jt_delta[par][indx];
} else {
dbg_img[3 * par + 0][pix] = Double.NaN;
dbg_img[3 * par + 1][pix] = Double.NaN;
dbg_img[3 * par + 2][pix] = Double.NaN;
}
}
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
width,
height,
true,
"Debug_Jacobians",
titles3);
}
return tot_error;
}
private void dbgYminusFx(
double [] fx,
String title) {
int gap = 10; //pix
int clusters = clustersX * clustersY;
String [] titles = {"Y", "-fX", "Y+fx"};
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
double [][] dbg_img = new double [3][width*height];
for (int mode = 0; mode < POINTS_SAMPLE; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX);
int pix = x + y * width;
int indx = cluster * POINTS_SAMPLE + mode;
if (measured_dsxy[cluster] != null){
if (mode ==0) {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF];
dbg_img[1][pix] = -fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DIFF] + fx[indx];
} else {
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1];
dbg_img[1][pix] = -fx[indx];
dbg_img[2][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_DD0 + mode - 1] + fx[indx];
}
} else {
dbg_img[ 0][pix] = Double.NaN;
dbg_img[ 1][pix] = Double.NaN;
dbg_img[ 2][pix] = Double.NaN;
}
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
width,
height,
true,
title,
titles);
}
private void dbgXY(
GeometryCorrection.CorrVector corr_vector,
String title) {
int gap = 10; //pix
int clusters = clustersX * clustersY;
String [] titles = {"meas", "correction", "diff"};
int width = 3 * clustersX + 2 * gap;
int height = 3 * clustersY + 2 * gap;
double [][] xy = getXYNondistorted(corr_vector, false);
double [][] dbg_img = new double [3][width*height];
for (int mode = 0; mode < 2 * NUM_SENSORS; mode++) {
int x0 = (mode % 3) * (clustersX + gap);
int y0 = (mode / 3) * (clustersY + gap);
for (int cluster = 0; cluster < clusters; cluster++) {
int x = x0 + (cluster % clustersX);
int y = y0 + (cluster / clustersX);
int pix = x + y * width;
// int indx = cluster * POINTS_SAMPLE + mode;
if (measured_dsxy[cluster] != null){
dbg_img[0][pix] = measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode];
dbg_img[1][pix] = xy[cluster][mode] - x0y0[cluster][mode];
dbg_img[2][pix] = xy[cluster][mode] - x0y0[cluster][mode] - measured_dsxy[cluster][ExtrinsicAdjustment.INDX_X0+mode];
} else {
dbg_img[ 0][pix] = Double.NaN;
dbg_img[ 1][pix] = Double.NaN;
dbg_img[ 2][pix] = Double.NaN;
}
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
width,
height,
true,
title,
titles);
}
private double [][] getJacobianTransposed(
GeometryCorrection.CorrVector corr_vector)
{
int clusters = clustersX * clustersY;
int num_pars = getNumPars();
double [][] jt = new double [num_pars][clusters * POINTS_SAMPLE ];
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
double [] imu = corr_vector.getIMU(); // i)
for (int cluster = 0; cluster < clusters; cluster++) if (measured_dsxy[cluster] != null){
// Mismatch mm = mismatch_list.get(indx);
// double [] pXY = mm.getPXY();
// will calculate 9 rows (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, columns - parameters
double [][] deriv = geometryCorrection.getPortsDDNDDerivatives( // USED in lwir
geometryCorrection, // GeometryCorrection gc_main,
use_rig_offsets, // boolean use_rig_offsets,
corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots,
dy_ddisparity[cluster], // double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu, // double [] imu,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 0], // double px,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_PX + 1], // double py,
measured_dsxy[cluster][ExtrinsicAdjustment.INDX_TARGET]); // double disparity);
/// int dbg_index = cluster; // dbg_index (pXY, dbg_decimate);
// convert to symmetrical coordinates
// derivatives of each port coordinates (in pixels) for each of selected symmetric all parameters (sym0 is convergence for disparity)
double [][] jt_partial = corr_vector.getJtPartial(
deriv, // double [][] port_coord_deriv,
par_mask); // boolean [] par_mask
// put partial (for 1 cluster 9 (disparity, dd0, dd1,cdd2, dd3, nd0, nd1, nd2, nd3}, transposed jacobian into full (all tiles) transposed Jacobian
for (int npar = 0; npar < jt.length; npar++){
for (int n = 0; n < jt_partial[npar].length; n++){
jt[npar][jt_partial[npar].length * cluster + n] = jt_partial[npar][n];
if (Double.isNaN(jt_partial[npar][n])) {
System.out.println("getJacobianTransposed(): npar="+npar+", cluster="+cluster+", n="+n);
}
}
}
}
return jt;
}
private double [][] getWJtJlambda( // USED in lwir
double lambda,
double [][] jt){
int num_pars = jt.length;
int nup_points = jt[0].length;
double [][] wjtjl = new double [num_pars][num_pars];
for (int i = 0; i < num_pars; i++) {
for (int j = i; j < num_pars; j++) {
double d = 0.0;
for (int k = 0; k < nup_points; k++) {
d += this.weights[k]*jt[i][k]*jt[j][k];
}
wjtjl[i][j] = d;
if (i == j) {
wjtjl[i][j] += d * lambda;
} else {
wjtjl[j][i] = d;
}
}
}
return wjtjl;
}
private boolean runLma(
double lambda, // 0.1
double lambda_scale_good,// 0.5
double lambda_scale_bad, // 8.0
double lambda_max, // 100
double rms_diff, // 0.001
int num_iter, // 20
int debug_level)
{
boolean [] rslt = {false,false};
this.last_rms = null;
int iter = 0;
for (iter = 0; iter < num_iter; iter++) {
rslt = lmaStep(
lambda,
rms_diff,
debug_level);
if (rslt == null) {
return false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (rslt[1]) {
break;
}
if (rslt[0]) { // good
lambda *= lambda_scale_good;
} else {
lambda *= lambda_scale_bad;
if (lambda > lambda_max) {
break; // not used in lwir
}
}
}
if (rslt[0]) { // better
if (iter >= num_iter) { // better, but num tries exceeded
if (debug_level > 0) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
} else {
if (debug_level > 0) System.out.println("Step "+iter+": LMA: Success");
}
} else { // improved over initial ?
if (last_rms[0] < initial_rms[0]) {
rslt[0] = true;
if (debug_level > 0) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
} else {
if (debug_level > 0) System.out.println("Step "+iter+": Failed to converge");
}
}
if (debug_level > 0) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
return rslt[0];
}
private int getNumPars() {
int num_pars = 0;
for (int i = 0; i < par_mask.length; i++) if (par_mask[i]) num_pars ++;
return num_pars;
}
// returns {success, done}
private boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
// int num_points = this.weights.length; // includes 2 extra for regularization
// int num_pars = getNumPars();
boolean [] rslt = {false,false};
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
// this.last_ymfx = getFxJt(
// this.vector, // double [] vector,
// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector);
if (debug_level > 2) {
dbgYminusFx(this.last_ymfx, "Initial y-fX");
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.last_rms = getWYmFxRms(
this.last_ymfx); // modifies this.last_ymfx (weights and subtracts fx
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
// TODO: Restore/implement
if (debug_level > 3) {
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
}
}
Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary
this.last_jt)); // double [][] jt)
if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv = null;
try {
jtjl_inv = wjtjlambda.inverse(); // check for errors
} catch (RuntimeException e) {
rslt[1] = true;
if (debug_level > 0) {
System.out.println("Singular Matrix!");
}
return rslt;
}
if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
//last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) {
System.out.println("mdelta");
mdelta.print(18, 6);
}
double [] delta = mdelta.getColumnPackedCopy();
GeometryCorrection.CorrVector corr_delta = geometryCorrection.getCorrVector(delta, par_mask);
/// double [] new_vector = this.vector.clone();
GeometryCorrection.CorrVector new_vector = this.corr_vector.clone();
double scale = 1.0;
// boolean ok =
new_vector.incrementVector(corr_delta, scale); // ok = false if there are nay NaN-s
/// for (int i = 0; i < num_pars; i++) new_vector[i]+= delta[i];
// being optimistic, modify jt and last_ymfx in place, restore if failed
/// this.last_ymfx = getFxJt(
/// new_vector, // double [] vector,
/// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(new_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(new_vector);
if (debug_level > 2) {
dbgYminusFx(this.last_ymfx, "next y-fX");
dbgXY(new_vector, "XY-correction");
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
double [] rms = getWYmFxRms(this.last_ymfx); // modifies this.last_ymfx
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
this.corr_vector = new_vector.clone();
if (debug_level > 2) {
System.out.print("New vector: "+new_vector.toString());
/// for (int np = 0; np < vector.length; np++) {
/// System.out.print(this.vector[np]+" ");
/// }
System.out.println();
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
/// this.last_ymfx = getFxJt( // recalculate fx
/// this.vector, // double [] vector,
/// this.last_jt); // double [][] jt) { // should be either [vector.length][samples.size()] or null - then only fx is calculated
this.last_jt = getJacobianTransposed(corr_vector); // new double [num_pars][num_points];
this.last_ymfx = getFx(corr_vector);
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.last_rms = getWYmFxRms(this.last_ymfx); // modifies this.last_ymfx
if (debug_level > 2) {
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
}
}
return rslt;
}
}
src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java
View file @ b47dba20
......@@ -41,8 +41,13 @@ public class GeometryCorrection {
public static String RIG_PREFIX = "rig-";
static double SCENE_UNITS_SCALE = 0.001; // meters from mm
static String SCENE_UNITS_NAME = "m";
static final String [] CORR_NAMES = {"tilt0","tilt1","tilt2","azimuth0","azimuth1","azimuth2","roll0","roll1","roll2","roll3","zoom0","zoom1","zoom2",
"omega_tilt", "omega_azimuth", "omega_roll"};
static final String [] CORR_NAMES = {
"tilt0","tilt1","tilt2",
"azimuth0","azimuth1","azimuth2",
"roll0","roll1","roll2","roll3",
"zoom0","zoom1","zoom2",
"omega_tilt", "omega_azimuth", "omega_roll",
"velocity_x", "velocity_y", "velocity_z"};
public int debugLevel = 0;
public double line_time = 26.5E-6; // duration of sensor scan line (for ERS)
......@@ -90,6 +95,11 @@ public class GeometryCorrection {
private double stepR=0.0001; // 0.001;
private double maxR=2.0; // calculate up to this*distortionRadius
private Matrix m_balance_xy = null; // [2*numSensors][2*numSensors] 8x8 matrix to make XY ports correction to have average == 0
private Matrix m_balance_dd = null; // [2*numSensors+1)][2*numSensors] 9x8 matrix to extract disparity from dd
public CorrVector extrinsic_corr;
public RigOffset rigOffset = null;
......@@ -108,6 +118,7 @@ public class GeometryCorrection {
public GeometryCorrection(double [] extrinsic_corr)
{
this.extrinsic_corr = new CorrVector(extrinsic_corr);
initPrePostMatrices(true); //false); //
}
public boolean isInitialized() {
......@@ -117,6 +128,60 @@ public class GeometryCorrection {
public double [][] getRXY(boolean use_rig){
return (use_rig && (rigOffset != null)) ? rigOffset.rXY_aux: rXY ;
}
public void initPrePostMatrices(boolean invert) {
double [][] a_balance_xy = new double [2*numSensors][2*numSensors];
double [][] a_balance_dd = new double [2*numSensors+1][2*numSensors];
double wsame = (numSensors - 1.0)/numSensors;
double wother = 1.0/numSensors;
double wsamei = invert ? -wsame : wsame;
double wotheri = invert ? -wother : wother;
for (int i = 0; i < numSensors; i++) {
for (int j = 0; j < numSensors; j++) {
a_balance_xy[2*i] [2*j] = (i == j)? wsamei: -wotheri;
a_balance_xy[2*i+1][2*j+1] = (i == j)? wsamei: -wotheri;
}
}
m_balance_xy = new Matrix(a_balance_xy);
for (int i = 0; i < numSensors; i++) {
a_balance_dd[0] [i] = wother;
for (int j = 0; j < numSensors; j++){
a_balance_dd[1 + i][j] = (j == i)? wsame: -wother;
}
a_balance_dd[numSensors + 1 + i][numSensors + i] = 1.0;
}
m_balance_dd = new Matrix(a_balance_dd);
}
public Matrix [] getRXYMatrix(boolean use_rig) {
double [][] rXY = getRXY(use_rig);
Matrix [] mrXY = new Matrix[rXY.length];
for (int c = 0; c < rXY.length; c++) {
double [][] am = {{rXY[c][0], - rXY[c][1]},{rXY[c][1], rXY[c][0]}};
mrXY[c] = new Matrix(am);
}
return mrXY;
}
/**
* Matrix to convert non-distorted {x0,y0,x1,y1,x2,y2,x3,y3} to {disp, dd0, dd1, dd2, dd3, nd0, nd1, nd2, nd3}
* Used for both values and derivatives by attitude angles, etc.
* @param use_rig
* @return
*/
public Matrix xyToDdnd(boolean use_rig) {
Matrix xyddnd = new Matrix(2*numSensors, 2*numSensors);
Matrix [] mrXY = getRXYMatrix(use_rig);
int [] rows = new int[2];
for (int c = 0; c < numSensors; c++) {
rows[0] = c;
rows[1] = c + numSensors;
xyddnd.setMatrix(rows, 2*c, 2*c+1, mrXY[c].inverse());
}
return m_balance_dd.times(xyddnd).times(m_balance_xy);
}
// public double [] getAuxOffset(boolean use_rig){
// double [] main_offset = {0.0,0.0};
......@@ -248,18 +313,42 @@ public class GeometryCorrection {
extrinsic_corr = new CorrVector();
}
public boolean [] getParMask(
public boolean [] getParMask( // for compatibility with old
// boolean disparity_only,
// boolean use_disparity,
boolean use_disparity,
// boolean use_other_extr,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength,
// boolean ers_rot, // Enable ERS correction of the camera rotation
// boolean ers_lin, // Enable ERS correction of the camera linear movement
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
return getParMask(
use_disparity,
use_aztilts, // Adjust azimuths and tilts excluding disparity
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,
corr_focalLength,
false, // boolean ers_rot, // Enable ERS correction of the camera rotation
false, // boolean ers_lin, // Enable ERS correction of the camera linear movement
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
}
public boolean [] getParMask(
boolean use_disparity,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength,
boolean ers_rot, // Enable ERS correction of the camera rotation
boolean ers_lin, // Enable ERS correction of the camera linear movement
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
......@@ -270,6 +359,8 @@ public class GeometryCorrection {
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,
corr_focalLength,
ers_rot, // Enable ERS correction of the camera rotation
ers_lin, // Enable ERS correction of the camera linear movement
manual_par_sel); // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
}
......@@ -1023,13 +1114,13 @@ public class GeometryCorrection {
public class CorrVector{
static final int LENGTH = 16; // 10;
static final int LENGTH = 19; // 10;
static final int LENGTH_ANGLES =10;
static final int TILT_INDEX = 0;
static final int AZIMUTH_INDEX = 3;
static final int ROLL_INDEX = 6;
static final int ZOOM_INDEX = 10;
static final int IMU_INDEX = 13; // d_tilt/dt (rad/s), d_az/dt, d_roll/dt for ERS correction
static final int IMU_INDEX = 13; // d_tilt/dt (rad/s), d_az/dt, d_roll/dt for ERS correction, dx/dt, dy/dt, dz/dt
static final double ROT_AZ_SGN = -1.0; // sign of first sin for azimuth rotation
static final double ROT_TL_SGN = 1.0; // sign of first sin for tilt rotation
static final double ROT_RL_SGN = 1.0; // sign of first sin for roll rotation
......@@ -1283,19 +1374,26 @@ public class GeometryCorrection {
}
public double [] getIMU() {
double [] imu = {vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2]};
double [] imu = {
vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],
vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
return imu;
}
public double [] getIMU(int chn) {
// considering all sensors parallel, if needed process all angles
double [] imu = {vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2]};
double [] imu = {
vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],
vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
return imu;
}
public void setIMU(double [] imu) {
vector [IMU_INDEX + 0] = imu[0];
vector [IMU_INDEX + 1] = imu[1];
vector [IMU_INDEX + 2] = imu[2];
vector [IMU_INDEX + 3] = imu[0];
vector [IMU_INDEX + 4] = imu[1];
vector [IMU_INDEX + 5] = imu[2];
}
......@@ -1352,7 +1450,8 @@ public class GeometryCorrection {
if (indx < LENGTH_ANGLES) return vector[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // rolls
if (indx < IMU_INDEX) return vector[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // zooms
if (indx < (IMU_INDEX+2)) return vector[indx]* (inPix? (1000.0*focalLength /pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < LENGTH) return vector[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // omega_roll
if (indx < (IMU_INDEX+3)) return vector[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // omega_roll
if (indx < LENGTH) return vector[indx]* (inPix? (1000.0): 1.0); // m/s or mm/s
return Double.NaN;
}
public double getExtrinsicSymParameterValue(int indx, boolean inPix) { // not used in lwir
......@@ -1362,7 +1461,8 @@ public class GeometryCorrection {
if (indx < LENGTH_ANGLES) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // rolls
if (indx < IMU_INDEX) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // zooms
if (indx < (IMU_INDEX+2)) return sym_vect[indx]* (inPix? (1000.0*focalLength /pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < LENGTH) return sym_vect[indx]* (inPix? (1000.0*distortionRadius/pixelSize): 1.0); // omega_roll
if (indx < (IMU_INDEX+3)) return sym_vect[indx]* (inPix? (1000.0*focalLength /pixelSize): 1.0); // omega_roll
if (indx < LENGTH) return sym_vect[indx]* (inPix? (1000.0): 1.0); // m/s mm/s
return Double.NaN;
}
......@@ -1390,10 +1490,14 @@ public class GeometryCorrection {
v[i] = vector[i]* 1000.0*focalLength/pixelSize; // omega_tilt and omega_azimuth
sv[i] = sym_vect[i]*1000.0*focalLength/pixelSize; // omega_tilt and omega_azimuth
}
for (int i = IMU_INDEX+2; i < LENGTH; i++){
for (int i = IMU_INDEX+2; i < IMU_INDEX+3; i++){
v[i] = vector[i]*1000.0*distortionRadius/pixelSize; // omega_roll
sv[i] = sym_vect[i]*1000.0*distortionRadius/pixelSize; // omega_rolls
}
for (int i = IMU_INDEX+3; i < LENGTH; i++){
v[i] = vector[i] *1000.0; // *distortionRadius/pixelSize; // movement mm/s
sv[i] = sym_vect[i]*1000.0; // *distortionRadius/pixelSize; // movement mm/s
}
s = String.format("tilt (up): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of the image center)\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) );
s += String.format("azimuth (right): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of the image center)\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) );
......@@ -1405,11 +1509,77 @@ public class GeometryCorrection {
s += String.format(" 0:%9.6fpx 1:%8.5fpx 2:%8.5fpx 3:%8.5fpx 4:%8.5fpx 5:%8.5fpx 6:%8.5fpx 7:%8.5fpx 8:%8.5fpx 9:%8.5fpx 10: %8.5fpx 11:%8.5fpx 12:%8.5fpx\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9], sv[10], sv[11], sv[12] );
s += String.format("omega_tilt = %9.4f pix/s, omega_azimuth = %9.4f pix/s, omega_roll = %9.4f pix/s\n", sv[13], sv[14], sv[15]);
s += String.format("omega_tilt = %9.4f pix/s, omega_azimuth = %9.4f pix/s, omega_roll = %9.4f pix/s\n", sv[13], sv[14], sv[15]);
s += String.format("velocity Vx = %9.4f mm/s, Vy = %9.4f mm/s, Vz = %9.4f mm/s\n", sv[16], sv[17], sv[18]);
return s;
}
public boolean editVector() {
System.out.println(toString());
String lines[] = toString().split("\\n");
double par_scales_tlaz = 1000.0*focalLength/pixelSize;
double par_scales_roll = 1000.0*distortionRadius/pixelSize;
double par_scales_linear = 1000.0;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Update extrinsic parameters",1500,1100);
for (String s:lines) {
gd.addMessage(s);
}
for (int i = 0; i < AZIMUTH_INDEX; i++){
gd.addNumericField("Tilt "+i+" (up)", par_scales_tlaz * vector[i], 10, 13,"pix",
"Vertical (around horizontal axis perpendicular to te camera view direction) rotations in pixels for current camera parameters");
}
for (int i = AZIMUTH_INDEX; i < ROLL_INDEX; i++){
gd.addNumericField("Azimuth " + (i - AZIMUTH_INDEX) + " (right)", par_scales_tlaz * vector[i], 10, 13,"pix",
"Horizontal (around vertical axis) rotations in pixels for current camera parameters");
}
for (int i = ROLL_INDEX; i < ZOOM_INDEX; i++){
gd.addNumericField("Roll " + (i - ROLL_INDEX) + " (clockwise)", par_scales_roll * vector[i] , 10, 13,"pix",
"Horizontal (around vertical axis) rotations in pixels for current camera parameters, 1 pixel corresponds to FoV edges");
}
for (int i = ZOOM_INDEX; i < IMU_INDEX; i++){
gd.addNumericField("Zoom " + (i - ZOOM_INDEX) + " (zoom in)", par_scales_roll * vector[i], 10, 13,"pix",
"Relative (to the average) focal length modifications, 1 pixel corresponds to FoV edges");
}
gd.addMessage("--- ERS correction parameters ---");
gd.addNumericField("Angular rotation azimuth (right)", par_scales_tlaz * vector[IMU_INDEX + 0], 10, 13,"pix/s",
"Horizonatal camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (up)", par_scales_tlaz * vector[IMU_INDEX + 1], 10, 13,"pix/s",
"Vertical camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (clockwise)", par_scales_roll * vector[IMU_INDEX + 2], 10, 13,"pix/s",
"Roll camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Linear velocity (right)", par_scales_linear * vector[IMU_INDEX + 3], 10, 13,"mm/s",
"Linear movement right");
gd.addNumericField("Linear velocity (up)", par_scales_linear * vector[IMU_INDEX + 4], 10, 13,"mm/s",
"Linear movement up");
gd.addNumericField("Linear velocity (forward)", par_scales_linear * vector[IMU_INDEX + 5], 10, 13,"mm/s",
"Linear movement forward");
gd.showDialog();
if (gd.wasCanceled()) return false;
for (int i = 0; i < ROLL_INDEX; i++){
vector[i] = gd.getNextNumber()/par_scales_tlaz;
}
for (int i = ROLL_INDEX; i < IMU_INDEX; i++){
vector[i] = gd.getNextNumber()/par_scales_roll;
}
vector[IMU_INDEX + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 2] = gd.getNextNumber()/par_scales_roll;
vector[IMU_INDEX + 3] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 4] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 5] = gd.getNextNumber()/par_scales_linear;
return true;
}
public String toStringDegrees() // not used in lwir
{
......@@ -1424,9 +1594,12 @@ public class GeometryCorrection {
} else if (i < IMU_INDEX){
v[i] = vector[i];
sv[i] = sym_vect[i];
} else {
} else if (i < IMU_INDEX+3){
v[i] = vector[i]*180/Math.PI;
sv[i] = sym_vect[i]*180/Math.PI;
} else {
v[i] = vector[i]*1000;
sv[i] = sym_vect[i]*1000;
}
}
......@@ -1441,7 +1614,9 @@ public class GeometryCorrection {
s += String.format(" 0:%9.6f° 1:%8.5f° 2:%8.5f° 3:%8.5f° 4:%8.5f° 5:%8.5f° 6:%8.5f° 7:%8.5f° 8:%8.5f° 9:%8.5f° 10: %8.5f‰ 11:%8.5f‰ 12:%8.5f‰\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9], 1000*sv[10], 1000*sv[11], 1000*sv[12] );
s += String.format("omega_tilt = %9.4°/s, omega_azimuth = %9.4°/s, omega_roll = %9.4°/s\n", sv[13], sv[14], sv[15]);
s += String.format("omega_tilt = %9.4°/s, omega_azimuth = %9.4°/s, omega_roll = %9.4°/s\n", sv[13], sv[14], sv[15]);
s += String.format("velocity Vx = %9.4f mm/s, Vy = %9.4f mm/s, Vz = %9.4f mm/s\n", sv[16], sv[17], sv[18]);
return s;
}
......@@ -1449,18 +1624,30 @@ public class GeometryCorrection {
public boolean editIMU() {
double par_scales_tlaz = 1000.0*focalLength/pixelSize;
double par_scales_roll = 1000.0*distortionRadius/pixelSize;
double par_scales_linear = 1000.0;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set camera angular velocities (in pix/sec)",800,300);
gd.addNumericField("Angular rotation azimuth (positive - rotate camera the right)", par_scales_tlaz * vector[IMU_INDEX + 0], 3,6,"pix/s",
gd.addNumericField("Angular rotation azimuth (positive - rotate camera the right)", par_scales_tlaz * vector[IMU_INDEX + 0], 3,6,"pix/s",
"Horizonatal camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (positive - rotate camera up)", par_scales_tlaz * vector[IMU_INDEX + 1], 3,6,"pix/s",
gd.addNumericField("Angular rotation tilt (positive - rotate camera up)", par_scales_tlaz * vector[IMU_INDEX + 1], 3,6,"pix/s",
"Vertical camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (positive - clockwise)", par_scales_roll * vector[IMU_INDEX + 2], 3,6,"pix/s",
"Rollc amera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (positive - clockwise)", par_scales_roll * vector[IMU_INDEX + 2], 3,6,"pix/s",
"Roll camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Linear velocity (positive - right)", par_scales_linear * vector[IMU_INDEX + 3], 3,6,"mm/s",
"Linear movement right");
gd.addNumericField("Linear velocity (positive - up)", par_scales_linear * vector[IMU_INDEX + 4], 3,6,"mm/s",
"Linear movement up");
gd.addNumericField("Linear velocity (positive - forward)", par_scales_linear * vector[IMU_INDEX + 5], 3,6,"mm/s",
"Linear movement forward");
gd.showDialog();
if (gd.wasCanceled()) return false;
vector[IMU_INDEX + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 2] = gd.getNextNumber()/par_scales_roll;
vector[IMU_INDEX + 3] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 4] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 5] = gd.getNextNumber()/par_scales_linear;
return true;
}
......@@ -1483,6 +1670,17 @@ public class GeometryCorrection {
return incrementVector(incr.toArray(), scale);
}
public CorrVector diffFromVector(CorrVector other_vector) {
double [] aother = other_vector.toArray();
double [] athis = toArray().clone();
for (int i = 0; i < athis.length; i++) {
athis[i] -= aother[i];
}
return new CorrVector(athis);
}
@Override
public CorrVector clone(){ // not used in lwir
return new CorrVector(this.vector.clone());
......@@ -1518,26 +1716,30 @@ public class GeometryCorrection {
public double [][] dSym_j_dTar_i() // USED in lwir
{
double [][] tar_to_sym = {
{-2.0, -2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.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, 0.0, 0.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, 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, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // a2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.5, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.5, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, -0.5, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, -0.5, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 3
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0}, // scale 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0}, // scale 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0}, // scale 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // omega_tilt
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // omega_azimuth
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0} // omega_roll
{-2.0, -2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // a2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.5, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.5, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, -0.5, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, -0.5, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // roll 3
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // scale 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // scale 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // scale 2
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // omega_tilt
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0}, // omega_azimuth
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, // omega_roll
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // velocity_x
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // velocity_y
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0} // velocity_z
};
return tar_to_sym;
......@@ -1579,32 +1781,37 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
*/
double [][] sym_to_tar= { // USED in lwir
// t0 t1 t2 a0 a1 a2 r0 r1 r2 r3 s0 s1 s2
{-0.125,-0.125, 0.125, 0.125,-0.125, 0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 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, 0.0, 0.0, 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, 0.0, 0.0, 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, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym5
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym6 = (r0+r1+r2+r3)/4
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym7 = (r0-r3)/2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym8 = (r1-r2)/2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, -1.0, -1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym9 = (r0+r3-r1-r2)/4
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, 0.5, -0.5, 0.0, 0.0, 0.0 }, // sym10 = -s0 - s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, -0.5, 0.5, 0.0, 0.0, 0.0 }, // sym11 = -s0 - s1
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, 0.5, 0.5, 0.0, 0.0, 0.0 }, // sym12 = s1 + s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // sym13 = omega_tilt
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // sym14 = omega_azimuth
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0} // sym15 = omega_roll
};
{-0.125,-0.125, 0.125, 0.125,-0.125, 0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym5
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym6 = (r0+r1+r2+r3)/4
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym7 = (r0-r3)/2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym8 = (r1-r2)/2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, -1.0, -1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym9 = (r0+r3-r1-r2)/4
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, 0.5, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym10 = -s0 - s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, -0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym11 = -s0 - s1
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, -1.0, 1.0, -0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym12 = s1 + s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym13 = omega_tilt
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 }, // sym14 = omega_azimuth
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 }, // sym15 = omega_roll
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }, // sym16 = velocity_x
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }, // sym17 = velocity_y
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 } // sym18 = velocity_z
} ;
return sym_to_tar;
}
/*
public boolean [] getParMask( // USED in lwir
boolean use_disparity,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength,
boolean ers_rot, // Enable ERS correction of the camera rotation
boolean ers_lin, // Enable ERS correction of the camera linear movement
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
......@@ -1617,14 +1824,15 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
false, // boolean corr_imu,
manual_par_sel);
}
*/
public boolean [] getParMask( // USED in lwir
boolean use_disparity,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength,
boolean corr_imu,
boolean ers_rot, // Enable ERS correction of the camera rotation
boolean ers_lin, // Enable ERS correction of the camera linear movement
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
......@@ -1642,9 +1850,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
corr_focalLength, //sym10
corr_focalLength, //sym11
corr_focalLength, //sym12
corr_imu, //sym13
corr_imu, //sym14
corr_imu //sym15
ers_rot, //sym13
ers_rot, //sym14
ers_rot, //sym15
ers_lin, //sym16
ers_lin, //sym17
ers_lin //sym18
};
if (manual_par_sel != 0) { // not used in lwir
for (int i = 0; i < par_mask.length; i++) {
......@@ -1701,6 +1912,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
}
return jt_part;
}
// convert tilt0,... roll3 array to symmetrical coordinates [0] - to the center (disparity)
public double [] toSymArray(boolean [] par_mask) // USED in lwir
{
......@@ -2673,6 +2885,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+1] = delta_t * rD2rND * dpYci_dazimuth * imu[0];
pXYderiv[2 * i + 0][CorrVector.IMU_INDEX+2] = delta_t * rD2rND * dpYci_droll * imu[0];
pXYderiv[2 * i + 1][CorrVector.IMU_INDEX+2] = delta_t * rD2rND * dpYci_droll * imu[0];
// TODO: Add linear egomotion
}
// verify that d/dsym are well, symmetrical
......@@ -2702,6 +2917,226 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
return pXY;
}
// private Matrix m_balance_xy = null; // [2*numSensors][2*numSensors] 8x8 matrix to make XY ports correction to have average == 0
// private Matrix m_balance_dd = null; // [2*numSensors+1)][2*numSensors] 9x8 matrix to extract disparity from dd
// calculate non-distorted x/y pairs (relative to optical centers) for each port and derivatives
public double [] getPortsDDNDAndDerivatives( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [][] DDNDderiv, // if not null, should be double[8][]
double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
double [] imu,
double [] pXYND0, // per-port non-distorted coordinates corresponding to the correlation measurements
double px,
double py,
double disparity)
{
// make sure initPrePostMatrices(true) already ran (in constructor). (true) means that minus sign is already incorporated
Matrix m_xy_ddnd = xyToDdnd(use_rig_offsets);
double [][] pXYNDderiv = (DDNDderiv == null)? null: new double [DDNDderiv.length][];
double [] pXYND = getPortsNonDistortedCoordinatesAndDerivatives( // USED in lwir
gc_main,
use_rig_offsets,
rots,
deriv_rots,
pXYNDderiv, // if not null, should be double[8][]
dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu,
px,
py,
disparity);
for (int i = 0; i < pXYND.length; i++) {
pXYND[i] -= pXYND0[i];
}
Matrix m_pXYND = new Matrix(pXYND,pXYND.length); // column
double [] dddnd = m_xy_ddnd.times(m_pXYND).getColumnPackedCopy();
if (pXYNDderiv != null) {
double [][] jt = m_xy_ddnd.times(new Matrix(pXYNDderiv)).getArray();
for (int i = 0; i < DDNDderiv.length; i++) {
DDNDderiv[i] = jt[i];
}
}
return dddnd;
}
// only derivatives
public double [][] getPortsDDNDDerivatives( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
double [] imu, // may be null
double px,
double py,
double disparity)
{
// make sure initPrePostMatrices(true) already ran (in constructor). (true) means that minus sign is already incorporated
Matrix m_xy_ddnd = xyToDdnd(use_rig_offsets);
double [][] pXYNDderiv = new double [2*numSensors][]; // CorrVector.LENGTH][];
getPortsNonDistortedCoordinatesAndDerivatives( // USED in lwir
gc_main,
use_rig_offsets,
rots,
deriv_rots,
pXYNDderiv, // if not null, should be double[8][]
dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
imu,
px,
py,
disparity);
double [][] jt = m_xy_ddnd.times(new Matrix(pXYNDderiv)).getArray();
return jt;
}
/**
* Calculate non-distorted x/y pairs (relative to optical centers) for each port and derivatives
* Requires array of derivatives of sesnor y by disparity ( calculated as disp_dist[i][2] by getPortsCoordinatesAndDerivatives())
* @param gc_main - GeometryCorrection instance for the main camera, for which px,py are specified
* @param use_rig_offsets - for the auxiliary camera - use offsets from the main one
* @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 pXYNDderiv null or double[2 * number_of_cameras][] array to accommodate derivatives of px, py by each of the parameters
* @param dy_ddisparity - array of per-port derivatives of sensor pY by disparity (to correct ERS) or null (if no ERS correction needed)
* @param px pixel X coordinate
* @param py pixel Y coordinate
* @param disparity disparity (for non-distorted image space)
* @return array of per port pairs of pixel shifts, non-distorted, relative to lens optical centers
*/
public double [] getPortsNonDistortedCoordinatesAndDerivatives( // USED in lwir
GeometryCorrection gc_main,
boolean use_rig_offsets,
Matrix [] rots,
Matrix [][] deriv_rots,
double [][] pXYNDderiv, // if not null, should be double[8][]
double [] dy_ddisparity, // double [][] disp_dist, //disp_dist[i][2] or null
double [] imu,
double px,
double py,
double disparity)
{
double [][] rXY = getRXY(use_rig_offsets); // may include rig offsets
double [] pXYND = new double [numSensors * 2];
double pXcd = px - 0.5 * gc_main.pixelCorrectionWidth;
double pYcd = py - 0.5 * gc_main.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*gc_main.pixelSize; // distorted radius in a virtual center camera
double rND2R=gc_main.getRByRDist(rD/gc_main.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
// next radial distortion coefficients are for this, not master camera (may be the same)
// double [] rad_coeff={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
double fl_pix = focalLength/(0.001*pixelSize); // focal length in pixels - this camera
// 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 * rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * rXY[i][1];
// rectilinear, end of dealing with possibly other (master) camera, below all is for this camera distortions
// Convert a 2-d non-distorted vector to 3d at fl_pix distance in z direction
double [][] avi = {{pXci0}, {pYci0},{fl_pix}};
Matrix vi = new Matrix(avi); // non-distorted sensor channel view vector in pixels (z -along the common axis)
// Apply port-individual combined rotation/zoom matrix
Matrix rvi = rots[i].times(vi);
// get back to the projection plane by normalizing vector
double norm_z = fl_pix/rvi.get(2, 0);
double pXci = rvi.get(0, 0) * norm_z;
double pYci = rvi.get(1, 0) * norm_z;
pXYND[2 * i + 0] = pXci;
pXYND[2 * i + 1] = pYci;
// used when calculating derivatives, TODO: combine calculations !
// double drD2rND_dri = 0.0;
Matrix drvi_daz = null;
Matrix drvi_dtl = null;
Matrix drvi_drl = null;
double dpXci_dazimuth = 0.0;
double dpYci_dazimuth = 0.0;
double dpXci_dtilt = 0.0;
double dpYci_dtilt = 0.0;
double dpXci_droll = 0.0;
double dpYci_droll = 0.0;
if (deriv_rots != null) {
// needed for derivatives and IMU
drvi_daz = deriv_rots[i][0].times(vi);
drvi_dtl = deriv_rots[i][1].times(vi);
drvi_drl = deriv_rots[i][2].times(vi);
dpXci_dazimuth = drvi_daz.get(0, 0) * norm_z - pXci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpYci_dazimuth = drvi_daz.get(1, 0) * norm_z - pYci * drvi_daz.get(2, 0) / rvi.get(2, 0);
dpXci_dtilt = drvi_dtl.get(0, 0) * norm_z - pXci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpYci_dtilt = drvi_dtl.get(1, 0) * norm_z - pYci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
dpXci_droll = drvi_drl.get(0, 0) * norm_z - pXci * drvi_drl.get(2, 0) / rvi.get(2, 0);
dpYci_droll = drvi_drl.get(1, 0) * norm_z - pYci * drvi_drl.get(2, 0) / rvi.get(2, 0);
}
double delta_t = 0.0;
if ((dy_ddisparity != null) && (imu != null)) {
delta_t = dy_ddisparity[i] * disparity * line_time; // positive for top cameras, negative - for bottom
double ers_Xci = delta_t* (dpXci_dtilt * imu[0] + dpXci_dazimuth * imu[1] + dpXci_droll * imu[2]);
double ers_Yci = delta_t* (dpYci_dtilt * imu[0] + dpYci_dazimuth * imu[1] + dpYci_droll * imu[2]);
pXYND[2 * i + 0] += ers_Xci; // added correction to pixel X
pXYND[2 * i + 1] += ers_Yci; // added correction to pixel Y
}
if (pXYNDderiv != null) {
pXYNDderiv[2 * i] = new double [CorrVector.LENGTH];
pXYNDderiv[2 * i+1] = new double [CorrVector.LENGTH];
Matrix drvi_dzm = deriv_rots[i][3].times(vi);
double dpXci_dzoom = drvi_dzm.get(0, 0) * norm_z - pXci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
double dpYci_dzoom = drvi_dzm.get(1, 0) * norm_z - pYci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
if (imu != null) {
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+0] = delta_t * dpXci_dtilt; // * imu[0];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+0] = delta_t * dpYci_dtilt; // * imu[0];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+1] = delta_t * dpXci_dazimuth; // * imu[1];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+1] = delta_t * dpYci_dazimuth; // * imu[1];
pXYNDderiv[2 * i + 0][CorrVector.IMU_INDEX+2] = delta_t * dpXci_droll; // * imu[2];
pXYNDderiv[2 * i + 1][CorrVector.IMU_INDEX+2] = delta_t * dpYci_droll; // * imu[2];
}
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYNDderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dpXci_dtilt;
pXYNDderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dpYci_dtilt;
pXYNDderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dpXci_dazimuth;
pXYNDderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dpYci_dazimuth;
pXYNDderiv[2 * i + 0][CorrVector.ZOOM_INDEX+i] = dpXci_dzoom;
pXYNDderiv[2 * i + 1][CorrVector.ZOOM_INDEX+i] = dpYci_dzoom;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYNDderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dpXci_dtilt;
pXYNDderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dpYci_dtilt;
pXYNDderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dpXci_dazimuth;
pXYNDderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dpYci_dazimuth;
pXYNDderiv[2 * i + 0][CorrVector.ZOOM_INDEX+j] = -dpXci_dzoom;
pXYNDderiv[2 * i + 1][CorrVector.ZOOM_INDEX+j] = -dpYci_dzoom;
}
}
pXYNDderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dpXci_droll;
pXYNDderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dpYci_droll;
}
}
return pXYND;
}
/**
* Calculate pixel common "idealized" coordinates of the auxiliary camera image tile matching the specified tile (px,py) of the idealized
* "center" (still distorted) image of the main camera and generic disparity, measured in pixels
......
src/main/java/com/elphel/imagej/tileprocessor/ImageDtt.java
View file @ b47dba20
......@@ -1808,6 +1808,8 @@ public class ImageDtt {
double [] strength = new double [clustSize];
// double [] disparity = new double [clustSize];
double [][] disp_str = new double [clustSize][];
double [][] dY_dD = new double [clustSize][quad];
double [][] pxpy = new double [clustSize][2];
boolean debugCluster = (clustX == debug_clustX) && (clustY == debug_clustY);
boolean debugCluster1 = (Math.abs(clustX - debug_clustX) < 10) && (Math.abs(clustY - debug_clustY) < 10);
......@@ -1947,6 +1949,8 @@ public class ImageDtt {
centerY,
disparity_array[tileY][tileX] + disparity_corr);
}
pxpy[cTile][0] = centerX;
pxpy[cTile][1] = centerY;
if (((globalDebugLevel > 0) || debug_distort) || (debugTile && (globalDebugLevel > -2))) {
for (int i = 0; i < quad; i++) {
......@@ -2257,7 +2261,8 @@ public class ImageDtt {
double [][] extra_stats = lma2.getTileStats();
// final double [][] lazy_eye_data = new double [clustersY*clustersX][];
// calculate average disparity per cluster using a sum of the disparity_array and the result of the LMA
double sum_wd = 0, sum_w = 0;
lazy_eye_data[nCluster] = new double [ExtrinsicAdjustment.INDX_LENGTH];
double sum_w = 0;
for (int cTileY = 0; cTileY < tileStep; cTileY++) {
tileY = clustY * tileStep + cTileY ;
if (tileY < tilesY) {
......@@ -2267,9 +2272,15 @@ public class ImageDtt {
cTile = cTileY * tileStep + cTileX;
tIndex = tileY * tilesX + tileX;
if ((lma_ds[cTile] != null) && (lma_ds[cTile][1]> 0.0)) {
double d = lma_ds[cTile][0] + disparity_array[tileY][tileX] + disparity_corr;
double w = lma_ds[cTile][1];
sum_wd += w * d;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP] += (lma_ds[cTile][0] + disparity_array[tileY][tileX] + disparity_corr) * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] += (disparity_array[tileY][tileX] + disparity_corr) * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF] += lma_ds[cTile][0] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] += pxpy[cTile][0] * w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] += pxpy[cTile][1] * w;
for (int cam = 0; cam < quad; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] += disp_dist[cTile][cam][2] * w;
}
sum_w += w;
}
}
......@@ -2277,20 +2288,34 @@ public class ImageDtt {
}
}
if (sum_w > 0.0) {
lazy_eye_data[nCluster] = new double [2+ 2 * ddnd.length];
lazy_eye_data[nCluster][0] = sum_wd / sum_w;
lazy_eye_data[nCluster][1] = stats[0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_STRENGTH] = stats[0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DISP] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_TARGET] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DIFF] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 0] /= sum_w;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_PX + 1] /= sum_w;
for (int cam = 0; cam < quad; cam++) {
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DYDDISP0 + cam] /= sum_w;
}
for (int cam = 0; cam < ddnd.length; cam++) {
if (ddnd[cam] != null) { //convert to x,y from dd/nd
lazy_eye_data[nCluster][2 * cam + 2] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data[nCluster][2 * cam + 3] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = ddnd[cam][0] * rXY[cam][0] - ddnd[cam][1] * rXY[cam][1];
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = ddnd[cam][0] * rXY[cam][1] + ddnd[cam][1] * rXY[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = ddnd[cam][0];
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = ddnd[cam][1];
} else {
lazy_eye_data[nCluster][2 * cam + 2] = Double.NaN;
lazy_eye_data[nCluster][2 * cam + 3] = 0.0;
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 0] = Double.NaN;
lazy_eye_data[nCluster][2 * cam + ExtrinsicAdjustment.INDX_X0 + 1] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_DD0 + cam] = Double.NaN;
lazy_eye_data[nCluster][ExtrinsicAdjustment.INDX_ND0 + cam] = Double.NaN;
}
}
} else {
lazy_eye_data[nCluster] = null;
}
// just for debugging, can be removed
/*
double [][] lma2_ds = lma2.lmaDisparityStrength(
imgdtt_params.lma_max_rel_rms, // maximal relative (to average max/min amplitude LMA RMS) // May be up to 0.3)
imgdtt_params.lma_min_strength, // minimal composite strength (sqrt(average amp squared over absolute RMS)
......@@ -2355,6 +2380,7 @@ public class ImageDtt {
}
}
}
*/
}
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ b47dba20
......@@ -3599,7 +3599,89 @@ public class QuadCLT {
IJ.d2s(0.000000001*(System.nanoTime()-this.startTime),3)+" sec, --- Free memory="+Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
}
public void processCLTQuadCorrsTest( // not used in lwir
public void processCLTQuadCorrsTestERS(
CLTParameters clt_parameters,
EyesisCorrectionParameters.DebayerParameters debayerParameters,
ColorProcParameters colorProcParameters,
CorrectionColorProc.ColorGainsParameters channelGainParameters,
EyesisCorrectionParameters.RGBParameters rgbParameters,
// int convolveFFTSize, // 128 - fft size, kernel size should be size/2
final boolean apply_corr, // calculate and apply additional fine geometry correction
final boolean infinity_corr, // calculate and apply geometry correction at infinity
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel)
{
if (infinity_corr && (clt_parameters.z_correction != 0.0)){
System.out.println(
"****************************************\n"+
"* Resetting manual infinity correction *\n"+
"****************************************\n");
clt_parameters.z_correction = 0.0;
}
this.startTime=System.nanoTime();
String [] sourceFiles=correctionsParameters.getSourcePaths();
SetChannels [] set_channels=setChannels(debugLevel);
if ((set_channels == null) || (set_channels.length==0)) {
System.out.println("No files to process (of "+sourceFiles.length+")");
return;
}
// multiply each image by this and divide by individual (if not NaN)
double [] referenceExposures = null;
if (!colorProcParameters.lwir_islwir) {
referenceExposures=eyesisCorrections.calcReferenceExposures(debugLevel);
}
for (int nSet = 0; nSet < set_channels.length; nSet++){
int [] channelFiles = set_channels[nSet].fileNumber();
boolean [][] saturation_imp = (clt_parameters.sat_level > 0.0)? new boolean[channelFiles.length][] : null;
double [] scaleExposures = new double[channelFiles.length];
ImagePlus [] imp_srcs = conditionImageSet(
clt_parameters, // EyesisCorrectionParameters.CLTParameters clt_parameters,
colorProcParameters,
sourceFiles, // String [] sourceFiles,
set_channels[nSet].name(), // String set_name,
referenceExposures, // double [] referenceExposures,
channelFiles, // int [] channelFiles,
scaleExposures, //output // double [] scaleExposures
saturation_imp, //output // boolean [][] saturation_imp,
debugLevel); // int debugLevel);
// once per quad here
processCLTQuadCorrTestERS( // returns ImagePlus, but it already should be saved/shown
imp_srcs, // [srcChannel], // should have properties "name"(base for saving results), "channel","path"
saturation_imp, // boolean [][] saturation_imp, // (near) saturated pixels or null
clt_parameters,
debayerParameters,
colorProcParameters,
channelGainParameters,
rgbParameters,
scaleExposures,
apply_corr, // calculate and apply additional fine geometry correction
infinity_corr, // calculate and apply geometry correction at infinity
threadsMax, // maximal number of threads to launch
updateStatus,
debugLevel);
//Runtime.getRuntime().gc();
if (debugLevel >-1) System.out.println("Processing set "+(nSet+1)+" (of "+set_channels.length+") finished at "+
IJ.d2s(0.000000001*(System.nanoTime()-this.startTime),3)+" sec, --- Free memory="+Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
if (eyesisCorrections.stopRequested.get()>0) {
System.out.println("User requested stop");
System.out.println("Processing "+(nSet + 1)+" file sets (of "+set_channels.length+") finished at "+
IJ.d2s(0.000000001*(System.nanoTime()-this.startTime),3)+" sec, --- Free memory="+Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
return;
}
}
System.out.println("processCLTQuadCorrs(): processing "+getTotalFiles(set_channels)+" files ("+set_channels.length+" file sets) finished at "+
IJ.d2s(0.000000001*(System.nanoTime()-this.startTime),3)+" sec, --- Free memory="+Runtime.getRuntime().freeMemory()+" (of "+Runtime.getRuntime().totalMemory()+")");
}
public void processCLTQuadCorrsTest(
CLTParameters clt_parameters,
EyesisCorrectionParameters.DebayerParameters debayerParameters,
ColorProcParameters colorProcParameters,
......@@ -4576,8 +4658,155 @@ public class QuadCLT {
return results;
}
public ImagePlus [] processCLTQuadCorrTestERS(
ImagePlus [] imp_quad, // should have properties "name"(base for saving results), "channel","path"
boolean [][] saturation_imp, // (near) saturated pixels or null
CLTParameters clt_parameters,
EyesisCorrectionParameters.DebayerParameters debayerParameters,
ColorProcParameters colorProcParameters,
CorrectionColorProc.ColorGainsParameters channelGainParameters,
EyesisCorrectionParameters.RGBParameters rgbParameters,
// int convolveFFTSize, // 128 - fft size, kernel size should be size/2
double [] scaleExposures, // probably not needed here
final boolean apply_corr, // calculate and apply additional fine geometry correction
final boolean infinity_corr, // calculate and apply geometry correction at infinity
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
int debugLevel){
if (debugLevel > -2) { // -1) {
debugLevel = clt_parameters.ly_debug_level;
}
final boolean batch_mode = clt_parameters.batch_run; //disable any debug images
String name=this.correctionsParameters.getModelName((String) imp_quad[0].getProperty("name"));
// int channel= Integer.parseInt((String) imp_src.getProperty("channel"));
String path= (String) imp_quad[0].getProperty("path");
ImagePlus [] results = new ImagePlus[imp_quad.length];
for (int i = 0; i < results.length; i++) {
results[i] = imp_quad[i];
results[i].setTitle(results[i].getTitle()+"RAW");
}
if (debugLevel>1) System.out.println("processing: "+path);
setTiles (imp_quad[0], // set global tp.tilesX, tp.tilesY
clt_parameters,
threadsMax);
// double [][] disparity_array = tp.setSameDisparity(clt_parameters.disparity); // 0.0); // [tp.tilesY][tp.tilesX] - individual per-tile expected disparity
ShowDoubleFloatArrays sdfa_instance = new ShowDoubleFloatArrays(); // just for debugging?
// clt_parameters.tileStep,
final int tilesX=tp.getTilesX(); // imp_quad[0].getWidth()/clt_parameters.transform_size;
final int tilesY=tp.getTilesY(); // imp_quad[0].getHeight()/clt_parameters.transform_size;
final int clustersX= (tilesX + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
final int clustersY= (tilesY + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
final double [][] lazy_eye_data = new double [clustersY*clustersX][];
public ImagePlus [] processCLTQuadCorrTest( // USED in lwir
// final int nTilesInChn=tilesX*tilesY;
final int nClustersInChn=clustersX * clustersY;
double [][] dsxy = new double[clustersX * clustersY][];
ImagePlus imp_sel = WindowManager.getCurrentImage();
if ((imp_sel == null) || (imp_sel.getStackSize() != ExtrinsicAdjustment.INDX_LENGTH)) {
System.out.println("No image / wrong image selected, bailing out");
return null;
} else {
System.out.println("Image: "+imp_sel.getTitle());
int width = imp_sel.getWidth();
int height = imp_sel.getHeight();
if ((width != clustersX) || (height != clustersY)) {
System.out.println(String.format("Image size mismatch: width=%d (%d), height=%d(%d)",
width, clustersX, height, clustersY));
return null;
}
ImageStack stack_sel = imp_sel.getStack();
float [][] fpixels = new float [ExtrinsicAdjustment.INDX_LENGTH][];
for (int i = 0; i < fpixels.length; i++) {
fpixels[i] = (float[]) stack_sel.getPixels(i+1);
}
for (int tile = 0; tile < dsxy.length; tile ++) {
if (fpixels[1][tile] > 0.0) {
dsxy[tile] = new double[fpixels.length];
for (int i = 0; i < fpixels.length; i++) {
dsxy[tile][i] = fpixels[i][tile];
}
}
}
}
ExtrinsicAdjustment ea = new ExtrinsicAdjustment(
geometryCorrection, // GeometryCorrection gc,
clt_parameters.tileStep, // int clusterSize,
clustersX, // int clustersX,
clustersY); // int clustersY);
double [] old_new_rms = new double[2];
boolean apply_extrinsic = (clt_parameters.ly_corr_scale != 0.0);
GeometryCorrection.CorrVector corr_vector = ea.solveCorr (
clt_parameters.ly_inf_en, // boolean use_disparity, // adjust disparity-related extrinsics
clt_parameters.ly_aztilt_en, // boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
clt_parameters.ly_diff_roll_en,//boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
// data, using just radial distortions
clt_parameters.ly_com_roll, //boolean common_roll, // Enable common roll (valid for high disparity range only)
clt_parameters.ly_focalLength, //boolean corr_focalLength, // Correct scales (focal length temperature? variations)
clt_parameters.ly_ers_rot, // boolean ers_rot, // Enable ERS correction of the camera rotation
clt_parameters.ly_ers_lin, // boolean ly_ers_lin, // Enable ERS correction of the camera linear movement
// add balancing-related here?
clt_parameters.ly_par_sel, // int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
1.0, // double weight_disparity,
1.0, // double weight_lazyeye,
dsxy, // double [][] measured_dsxy,
null, // boolean [] force_disparity, // boolean [] force_disparity,
geometryCorrection, // GeometryCorrection geometryCorrection,
false, // boolean use_main, // corr_rots_aux != null;
geometryCorrection.getCorrVector(), // GeometryCorrection.CorrVector corr_vector,
old_new_rms, // double [] old_new_rms, // should be double[2]
debugLevel + 5);// int debugLevel)
if (debugLevel > -1){
System.out.println("Old extrinsic corrections:");
System.out.println(geometryCorrection.getCorrVector().toString());
}
if (corr_vector != null) {
GeometryCorrection.CorrVector diff_corr = corr_vector.diffFromVector(geometryCorrection.getCorrVector());
if (debugLevel > -1){
System.out.println("New extrinsic corrections:");
System.out.println(corr_vector.toString());
System.out.println("Increment extrinsic corrections:");
System.out.println(diff_corr.toString());
// System.out.println("Correction scale = "+clt_parameters.ly_corr_scale);
}
if (apply_extrinsic){
geometryCorrection.setCorrVector(corr_vector) ;
/*
geometryCorrection.getCorrVector().incrementVector(diff_corr, clt_parameters.ly_corr_scale);
System.out.println("New (with correction scale applied) extrinsic corrections:");
System.out.println(geometryCorrection.getCorrVector().toString());
*/
System.out.println("Extrinsic correction updated (can be disabled by setting clt_parameters.ly_corr_scale = 0.0) ");
} else {
System.out.println("Correction is not applied according clt_parameters.ly_corr_scale == 0.0) ");
}
} else {
if (debugLevel > -2){
System.out.println("LMA failed");
}
}
double [][][] mismatch_corr_coefficients = new double [1][2][];
mismatch_corr_coefficients[0][0] = geometryCorrection.getCorrVector().toSymArray(null);
mismatch_corr_coefficients[0][1] = old_new_rms;
return null;
}
public ImagePlus [] processCLTQuadCorrTest(
ImagePlus [] imp_quad, // should have properties "name"(base for saving results), "channel","path"
boolean [][] saturation_imp, // (near) saturated pixels or null
CLTParameters clt_parameters,
......@@ -4783,38 +5012,25 @@ public class QuadCLT {
debugLevel);
if (lazy_eye_data != null) {
int ns = 0;
for (int n = 0; n < lazy_eye_data.length; n++) {
if (lazy_eye_data[n] != null) {
ns = lazy_eye_data[n].length;
break;
}
}
if (ns > 0) {
String [] titles = new String [ns];
titles [0] = "disparity";
titles [1] = "strength";
for (int i = 0; i < (ns - 2)/2; i++) {
titles [2*i + 2] = "dx-"+i;
titles [2*i + 3] = "dy-"+i;
}
int clustersX= (tilesX + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
int clustersY= (tilesY + clt_parameters.tileStep - 1) / clt_parameters.tileStep;
double [][] dbg_cluster = new double [ns][clustersY * clustersX];
double [][] dbg_cluster = new double [ExtrinsicAdjustment.INDX_LENGTH][clustersY * clustersX];
for (int n = 0; n < lazy_eye_data.length; n++) {
if ((lazy_eye_data[n] != null) && (lazy_eye_data[n][1] >= clt_parameters.img_dtt.lma_diff_minw)) {
dbg_cluster[0][n] = lazy_eye_data[n][0]; // disparity
dbg_cluster[1][n] = lazy_eye_data[n][1] - clt_parameters.img_dtt.lma_diff_minw; // strength
for (int i = 0; i < (ns - 2)/2; i++) {
dbg_cluster[2 * i + 2][n] = lazy_eye_data[n][2 * i + 2]; // x
dbg_cluster[2 * i + 3][n] = lazy_eye_data[n][2 * i + 3]; // y
if ((lazy_eye_data[n] != null) && (lazy_eye_data[n][ExtrinsicAdjustment.INDX_STRENGTH] >= clt_parameters.img_dtt.lma_diff_minw)) {
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++ ) {
if (i == ExtrinsicAdjustment.INDX_STRENGTH) {
dbg_cluster[i][n] = lazy_eye_data[n][i] - clt_parameters.img_dtt.lma_diff_minw; // strength
} else {
dbg_cluster[i][n] = lazy_eye_data[n][i];
}
}
} else {
dbg_cluster[0][n] = Double.NaN;
dbg_cluster[1][n] = 0.0;
for (int i = 0; i < (ns - 2)/2; i++) {
dbg_cluster[2 * i + 2][n] = Double.NaN; // x
dbg_cluster[2 * i + 3][n] = Double.NaN; // y
for (int i = 0; i < ExtrinsicAdjustment.INDX_LENGTH; i++ ) {
if (i == ExtrinsicAdjustment.INDX_STRENGTH) {
dbg_cluster[i][n] = 0.0; // strength
} else {
dbg_cluster[i][n] = Double.NaN;
}
}
}
}
......@@ -4825,10 +5041,10 @@ public class QuadCLT {
clustersY,
true,
name+sAux()+"-CLT_MISMATCH-D"+clt_parameters.disparity+"_"+clt_parameters.tileStep+"x"+clt_parameters.tileStep,
titles);
}
ExtrinsicAdjustment.DATA_TITLES);
}
/*
if (disparity_map != null){
if (!batch_mode && clt_parameters.show_map && (debugLevel > -2)){
sdfa_instance.showArrays(
......@@ -4920,16 +5136,8 @@ public class QuadCLT {
true,
name+sAux()+"-CLT_MISMATCH-BLUR-D"+clt_parameters.disparity);
}
/*
public boolean lma_diff_xy = true; // convert dd/nd to x,y
public double lma_diff_minw = 0.5; // minimal weight to keep
public double lma_diff_sigma = 2.0; // blur differential data (relative to the cluster linear size)
*/
}
*/
return results;
}
......@@ -5488,9 +5696,9 @@ public class QuadCLT {
{
if (geometryCorrection == null){
System.out.println("are not set, will be:");
return new GeometryCorrection(this.extrinsic_vect).getCorrVector().editIMU();
return new GeometryCorrection(this.extrinsic_vect).getCorrVector().editVector(); // editIMU();
} else {
return geometryCorrection.getCorrVector().editIMU();
return geometryCorrection.getCorrVector().editVector(); // .editIMU();
}
}
......
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