Skip to content

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

Implemented/tested zoom correction and simple (static) tabbed 

replacement for IJ GenericDialog
parent ce517a59
Hide whitespace changes
Inline Side-by-side
Showing with 2917 additions and 1462 deletions
src/main/java/AlignmentCorrection.java
View file @ 3f7a67de
...@@ -622,16 +622,7 @@ public class AlignmentCorrection { ...@@ -622,16 +622,7 @@ public class AlignmentCorrection {
int tileY = s.tile / tilesX; int tileY = s.tile / tilesX;
double centerX = tileX * qc.tp.getTileSize() + qc.tp.getTileSize()/2;// - shiftX; double centerX = tileX * qc.tp.getTileSize() + qc.tp.getTileSize()/2;// - shiftX;
double centerY = tileY * qc.tp.getTileSize() + qc.tp.getTileSize()/2;//- shiftY; double centerY = tileY * qc.tp.getTileSize() + qc.tp.getTileSize()/2;//- shiftY;
/*
double [][] centersXY_disp = qc.geometryCorrection.getPortsCoordinates(
centerX,
centerY,
disp_strength[2 * s.series + 0][s.tile]/magic_coeff); // disparity
double [][] centersXY_inf = qc.geometryCorrection.getPortsCoordinates(
centerX,
centerY,
0.0); // disparity
*/
double [][] centersXY_disp = qc.geometryCorrection.getPortsCoordinatesAndDerivatives( double [][] centersXY_disp = qc.geometryCorrection.getPortsCoordinatesAndDerivatives(
corr_rots, // Matrix [] rots, corr_rots, // Matrix [] rots,
null, // Matrix [][] deriv_rots, null, // Matrix [][] deriv_rots,
...@@ -2094,6 +2085,7 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2094,6 +2085,7 @@ B = |+dy0 -dy1 -2*dy3 |
clt_parameters.ly_inf_en, // boolean use_disparity, // if true will ignore disparity data even if available (was false) clt_parameters.ly_inf_en, // boolean use_disparity, // if true will ignore disparity data even if available (was false)
clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
clt_parameters.ly_com_roll, // boolean common_roll, // Enable common roll (valid for high disparity range only) 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)
mismatch_list, // ArrayList<Mismatch> mismatch_list, mismatch_list, // ArrayList<Mismatch> mismatch_list,
qc.geometryCorrection, // GeometryCorrection geometryCorrection, qc.geometryCorrection, // GeometryCorrection geometryCorrection,
...@@ -2250,15 +2242,6 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2250,15 +2242,6 @@ B = |+dy0 -dy1 -2*dy3 |
double [] pXY = mm.getPXY(); double [] pXY = mm.getPXY();
double [][] deriv = new double [2 * NUM_SENSORS][]; double [][] deriv = new double [2 * NUM_SENSORS][];
int dbg_index =dbg_index (pXY, dbg_decimate); int dbg_index =dbg_index (pXY, dbg_decimate);
// double [][] f =
/*
geometryCorrection.getPortsCoordinatesAndDerivatives_old(
corr_vector, // CorrVector corr_vector,
deriv, // boolean calc_deriv,
pXY[0], // double px,
pXY[1], // double py,
mm.getDisparityMeas()); // getDisparityTask()); // double disparity)
*/
geometryCorrection.getPortsCoordinatesAndDerivatives( geometryCorrection.getPortsCoordinatesAndDerivatives(
corr_rots, // Matrix [] rots, corr_rots, // Matrix [] rots,
deriv_rots, // Matrix [][] deriv_rots, deriv_rots, // Matrix [][] deriv_rots,
...@@ -2421,14 +2404,6 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2421,14 +2404,6 @@ B = |+dy0 -dy1 -2*dy3 |
for (int indx = 0; indx<mismatch_list.size(); indx++){ // need indx value for (int indx = 0; indx<mismatch_list.size(); indx++){ // need indx value
Mismatch mm = mismatch_list.get(indx); Mismatch mm = mismatch_list.get(indx);
double [] pXY = mm.getPXY(); double [] pXY = mm.getPXY();
/*
double [][] f = geometryCorrection.getPortsCoordinatesAndDerivatives_old( // 4x2
corr_vector, // CorrVector corr_vector,
null, // boolean calc_deriv,
pXY[0], // double px,
pXY[1], // double py,
mm.getDisparityMeas()); // getDisparityTask()); // double disparity)
*/
double [][] f = geometryCorrection.getPortsCoordinatesAndDerivatives( // 4x2 double [][] f = geometryCorrection.getPortsCoordinatesAndDerivatives( // 4x2
corr_rots, // Matrix [] rots, corr_rots, // Matrix [] rots,
null, // Matrix [][] deriv_rots, null, // Matrix [][] deriv_rots,
...@@ -2593,6 +2568,8 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2593,6 +2568,8 @@ B = |+dy0 -dy1 -2*dy3 |
boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
// data, using just radial distortions // data, using just radial distortions
boolean common_roll, // Enable common roll (valid for high disparity range only) boolean common_roll, // Enable common roll (valid for high disparity range only)
boolean corr_focalLength, // Correct scales (focal length temperature? variations)
ArrayList<Mismatch> mismatch_list, ArrayList<Mismatch> mismatch_list,
GeometryCorrection geometryCorrection, GeometryCorrection geometryCorrection,
...@@ -2608,35 +2585,21 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2608,35 +2585,21 @@ B = |+dy0 -dy1 -2*dy3 |
break; break;
} }
} }
// boolean [] par_mask = new boolean[10];
// for (int i = (has_disparity ? 0 : 1); i < par_mask.length; i++){
// par_mask[i] = true;
// }
boolean [] par_mask = geometryCorrection.getParMask( boolean [] par_mask = geometryCorrection.getParMask(
has_disparity, // boolean use_disparity, has_disparity, // boolean use_disparity,
common_roll); // boolean common_roll); common_roll,// boolean common_roll,
/* { // TODO: move to GeometryCorrection corr_focalLength); // boolean corr_focalLength);
has_disparity, //sym0
true, //sym1
true, //sym2
true, //sym3
true, //sym4
true, //sym5
common_roll, //sym6 // common roll
true, //sym7
true, //sym8
true //sym9
};
*/
double [][] jta = getJacobianTransposed( double [][] jta = getJacobianTransposed(
par_mask, // boolean [] par_mask, par_mask, // boolean [] par_mask,
mismatch_list, // ArrayList<Mismatch> mismatch_list, mismatch_list, // ArrayList<Mismatch> mismatch_list,
geometryCorrection, // GeometryCorrection geometryCorrection, geometryCorrection, // GeometryCorrection geometryCorrection,
corr_vector, // GeometryCorrection.CorrVector corr_vector) corr_vector, // GeometryCorrection.CorrVector corr_vector)
// debugLevel); // int debugLevel)
debugLevel); // int debugLevel) debugLevel); // int debugLevel)
// debugLevel = 2;
// convert Jacobian outputs to symmetrical measurement vectors (last one is non-zero only if disparity should be adjusted) // convert Jacobian outputs to symmetrical measurement vectors (last one is non-zero only if disparity should be adjusted)
...@@ -2653,7 +2616,7 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2653,7 +2616,7 @@ B = |+dy0 -dy1 -2*dy3 |
mismatch_list); // ArrayList<Mismatch> mismatch_list) mismatch_list); // ArrayList<Mismatch> mismatch_list)
double [] y_minus_fx_a_weighted = mulWeight(y_minus_fx_a, weights); double [] y_minus_fx_a_weighted = mulWeight(y_minus_fx_a, weights);
double rms0 = getRMS (y_minus_fx_a, weights); double rms0 = getRMS (y_minus_fx_a, weights);
if (debugLevel > -1){ if (debugLevel > -2){
System.out.println("--- solveCorr(): initial RMS = " + rms0); System.out.println("--- solveCorr(): initial RMS = " + rms0);
} }
...@@ -2661,14 +2624,8 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2661,14 +2624,8 @@ B = |+dy0 -dy1 -2*dy3 |
old_new_rms[0] =rms0; old_new_rms[0] =rms0;
} }
Matrix y_minus_fx_weighted = new Matrix(y_minus_fx_a_weighted, y_minus_fx_a_weighted.length); Matrix y_minus_fx_weighted = new Matrix(y_minus_fx_a_weighted, y_minus_fx_a_weighted.length);
// double [][] jtja = getJTJ(jta, weights);
double [][] jtja = getJTJ(jta_mv, weights); double [][] jtja = getJTJ(jta_mv, weights);
Matrix jtj = new Matrix(jtja); // getJTJ(jta, weights)); // less operations than jt.times(jt.transpose()); Matrix jtj = new Matrix(jtja); // getJTJ(jta, weights)); // less operations than jt.times(jt.transpose());
// double [] jt_trace_null_dbg = getJtJTrace(jta,null);
// double [] jt_trace_dbg = getJtJTrace(jta,weights);
// double [] jt_mv_trace_null_dbg = getJtJTrace(jta_mv,null);
// double [] jt_mv_trace_dbg = getJtJTrace(jta_mv,weights);
//
boolean dbg_images = debugLevel>1; boolean dbg_images = debugLevel>1;
int dbg_decimate = 64; // just for the debug image int dbg_decimate = 64; // just for the debug image
int dbg_width = qc.tp.getTilesX()*qc.tp.getTileSize(); int dbg_width = qc.tp.getTilesX()*qc.tp.getTileSize();
...@@ -2676,8 +2633,7 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2676,8 +2633,7 @@ B = |+dy0 -dy1 -2*dy3 |
int dbg_owidth = dbg_width/dbg_decimate; int dbg_owidth = dbg_width/dbg_decimate;
int dbg_oheight = dbg_height/dbg_decimate; int dbg_oheight = dbg_height/dbg_decimate;
int dbg_length = dbg_owidth*dbg_oheight; int dbg_length = dbg_owidth*dbg_oheight;
// String [] dbg_titles_tar=GeometryCorrection.CORR_NAMES; String [] dbg_titles_sym= {"sym0","sym1","sym2","sym3","sym4","sym5","sroll0","sroll1","sroll2","sroll3", "zoom0", "zoom1", "zoom2"};
String [] dbg_titles_sym= {"sym0","sym1","sym2","sym3","sym4","sym5","sroll0","sroll1","sroll2","sroll3"};
String [] dbg_titles_xy= {"x0","y0","x1","y1","x2","y2","x3","y3"}; String [] dbg_titles_xy= {"x0","y0","x1","y1","x2","y2","x3","y3"};
String [] dbg_titles_mv= {"dy0","dy1","dx2","dx3","dx1-dx0","dy3-dy2","dh-dv","dhy+dv"}; String [] dbg_titles_mv= {"dy0","dy1","dx2","dx3","dx1-dx0","dy3-dy2","dh-dv","dhy+dv"};
double [][] dbg_xy = null; // jacobian dmv/dsym double [][] dbg_xy = null; // jacobian dmv/dsym
...@@ -2699,8 +2655,6 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2699,8 +2655,6 @@ B = |+dy0 -dy1 -2*dy3 |
dbg_dmv_dsym = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym dbg_dmv_dsym = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym
dbg_dmv_dsym_delta = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym dbg_dmv_dsym_delta = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym
dbg_dmv_dsym_diff = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym dbg_dmv_dsym_diff = doubleNaN(dbg_titles_mv.length * dbg_titles_sym.length, dbg_length); // jacobian dmv/dsym
// dbg_xy = new double [dbg_titles_xy.length] [dbg_length]; // jacobian dmv/dsym
// dbg_mv = new double [dbg_titles_mv.length] [dbg_length]; // jacobian dmv/dsym
String [] dbg_dmv_dsym_titles = new String [dbg_titles_mv.length * dbg_titles_sym.length]; String [] dbg_dmv_dsym_titles = new String [dbg_titles_mv.length * dbg_titles_sym.length];
for (int i = 0; i < dbg_titles_mv.length; i++){ for (int i = 0; i < dbg_titles_mv.length; i++){
...@@ -2764,7 +2718,7 @@ B = |+dy0 -dy1 -2*dy3 | ...@@ -2764,7 +2718,7 @@ B = |+dy0 -dy1 -2*dy3 |
drslt[i] *= -1.0; drslt[i] *= -1.0;
} }
GeometryCorrection.CorrVector rslt = geometryCorrection.getCorrVector(drslt, par_mask); GeometryCorrection.CorrVector rslt = geometryCorrection.getCorrVector(drslt, par_mask);
if (debugLevel > -1){ if (debugLevel > -2){
System.out.println("solveCorr() rslt:"); System.out.println("solveCorr() rslt:");
System.out.println(rslt.toString()); System.out.println(rslt.toString());
} }
......
src/main/java/EyesisCorrectionParameters.java
View file @ 3f7a67de
This source diff could not be displayed because it is too large. You can view the blob instead.
src/main/java/Eyesis_Correction.java
View file @ 3f7a67de
...@@ -568,10 +568,13 @@ private Panel panel1, ...@@ -568,10 +568,13 @@ private Panel panel1,
if (DCT_MODE) { if (DCT_MODE) {
panelClt3 = new Panel(); panelClt3 = new Panel();
panelClt3.setLayout(new GridLayout(1, 0, 5, 5)); // rows, columns, vgap, hgap panelClt3.setLayout(new GridLayout(1, 0, 5, 5)); // rows, columns, vgap, hgap
addButton("Setup CLT parameters", panelClt3, color_configure); addButton("Setup CLT", panelClt3, color_configure);
addButton("Infinity offset", panelClt3, color_configure); addButton("Infinity offset", panelClt3, color_configure);
addButton("Setup CLT Batch parameters", panelClt3, color_configure); addButton("Setup CLT Batch parameters", panelClt3, color_configure);
addButton("CLT batch process", panelClt3, color_process); addButton("CLT batch process", panelClt3, color_process);
// addButton("JTabbed", panelClt3, color_stop);
// addButton("Demo", panelClt3, color_process);
add(panelClt3); add(panelClt3);
} }
pack(); pack();
...@@ -1148,7 +1151,7 @@ private Panel panel1, ...@@ -1148,7 +1151,7 @@ private Panel panel1,
/* ======================================================================== */ /* ======================================================================== */
} else if (label.equals("Configure correction")) { } else if (label.equals("Configure correction")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL; DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
CORRECTION_PARAMETERS.showDialog("Correction parameters"); CORRECTION_PARAMETERS.showJDialog("Correction parameters");
return; return;
...@@ -3751,6 +3754,10 @@ private Panel panel1, ...@@ -3751,6 +3754,10 @@ private Panel panel1,
} else if (label.equals("Setup CLT parameters")) { } else if (label.equals("Setup CLT parameters")) {
CLT_PARAMETERS.showDialog(); CLT_PARAMETERS.showDialog();
return; return;
/* ======================================================================== */
} else if (label.equals("Setup CLT")) {
CLT_PARAMETERS.showJDialog();
return;
/* ======================================================================== */ /* ======================================================================== */
} else if (label.equals("Infinity offset")) { } else if (label.equals("Infinity offset")) {
while (true) { while (true) {
...@@ -5155,11 +5162,7 @@ private Panel panel1, ...@@ -5155,11 +5162,7 @@ private Panel panel1,
} }
return; return;
//JTabbedTest
//
// End of buttons code // End of buttons code
} }
DEBUG_LEVEL=MASTER_DEBUG_LEVEL; DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
......
src/main/java/GenericJTabbedDialog.java 0 → 100644
View file @ 3f7a67de
/*
* TabbedPaneDemo.java requires one additional file:
* images/middle.gif.
*/
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.FlowLayout;
import java.awt.Font;
import java.awt.GridBagConstraints;
import java.awt.GridBagLayout;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.util.ArrayList;
import javax.swing.BorderFactory;
import javax.swing.JButton;
import javax.swing.JCheckBox;
import javax.swing.JComboBox;
import javax.swing.JComponent;
import javax.swing.JDialog;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.JScrollPane;
import javax.swing.JTabbedPane;
import javax.swing.JTextField;
import javax.swing.border.Border;
import ij.IJ;
public class GenericJTabbedDialog implements ActionListener {
static final int DIVIDE_COLUMNS_BY = 2; // JTextField.setColumns() seem to make it twice wider?
static final int DEFAULT_STRING_WIDTH = 8;
static final Color COMMENT_COLOR = new Color(0, 150, 0);
static final int COMMENT_PADY = 20; // make comment/message line wider
//0: label:'LabelUI', input:'TextFieldUI', inp_units:'PanelUI'
ArrayList<ArrayList<JComponent>> components = new ArrayList<ArrayList<JComponent>>(); // null component means a message, not am input
ArrayList<ArrayList<JLabel>> labels = new ArrayList<ArrayList<JLabel>>();
ArrayList<JComponent> tabs = new ArrayList<JComponent>();
ArrayList<JButton> buttons = new ArrayList<JButton>();
ArrayList<JScrollPane> scrollPanes = new ArrayList<JScrollPane>();
private JFrame frame = new JFrame();
public int width, height;
private int read_tab=0,read_component=0; // current number of tab/component to be read next
String result=null;
private JDialog jd;
public GenericJTabbedDialog(String title) {
this(title, 600, 800);
}
public GenericJTabbedDialog(String title, int width, int height) {
// final JFrame frame= new JFrame();
jd = new JDialog(frame , title, true);
components.add(new ArrayList<JComponent>());
labels.add(new ArrayList<JLabel>());
tabs.add (new JPanel(false)); // first (yet nameless tab)
this.width = width;
this.height = height;
}
public void addTab(String tab_name) {
addTab(tab_name, "");
}
public void addTab(String tab_name, String tab_tooltip) {
// See if there are any components for the current tab. If none (such as when none are yet added, just rename existing)
// If there are some components already - start a new tab
int ntab = tabs.size()-1;
if (!components.get(ntab).isEmpty()) {
components.add(new ArrayList<JComponent>());
labels.add(new ArrayList<JLabel>());
tabs.add (new JPanel(false)); // starting next tab
ntab++;
}
tabs.get(ntab).putClientProperty("title", tab_name); // to be later used for tab name
tabs.get(ntab).putClientProperty("tooltip",tab_tooltip); // to be later used for tab tooltip
}
public void addButtons(
String [] labels,
String [] actions,
String [] tooltips) {
for (int i = 0; i < labels.length; i++) {
JButton button = new JButton(labels[i]);
if ((actions != null) && (actions[i] != null)) button.setActionCommand(actions[i]);
if ((tooltips != null) && (tooltips[i] != null)) button.setToolTipText(tooltips[i]);
buttons.add(button);
}
}
public void addDefaultButtons() {
String [] labels = {"OK", "Cancel"};
addButtons(labels, labels, labels);
}
// Common part of adding messages/input fields
private void addLine(String prompt,
JComponent component, // may be null
String tooltip) {
JLabel label;
if (prompt.indexOf('_') == -1) {
label = new JLabel(prompt);
} else {
label = new JLabel(prompt.replace('_', ' '));
}
if (tooltip != null) {
label.setToolTipText(tooltip);
if (component != null) component.setToolTipText(tooltip);
}
labels.get(labels.size()-1).add(label);
components.get(components.size()-1).add(component);
}
public void addMessage(String message) {
addMessage(message, null);
}
public void addMessage(String message, String tooltip) {
addLine(message, null, tooltip);
}
public void addStringField (String label, String value) {
addStringField(label, value, DEFAULT_STRING_WIDTH, null);
}
public void addStringField (String label, String value, String tooltip) {
addStringField(label, value, DEFAULT_STRING_WIDTH, tooltip);
}
public void addStringField (String label, String value, int width) {
addStringField(label, value, width, null);
}
public void addStringField ( // no units here
String label,
String value,
int width,
String tooltip) {
JTextField inp = new JTextField();
inp.setText(value);
inp.setColumns(width/DIVIDE_COLUMNS_BY + 2); ///2);
JPanel inp_units = new JPanel(false);
inp_units.add(inp);
inp_units.putClientProperty("type", "String");
inp_units.setLayout(new FlowLayout(FlowLayout.LEFT));
addLine(label, inp_units, tooltip);
}
public void addNumericField(String label, double defaultValue, int digits) { // as in IJ
addNumericField(label, defaultValue, digits, 6, null, null);
}
public void addNumericField(String label, double defaultValue, int digits, int columns, String units) { // as in IJ
addNumericField(label, defaultValue, digits, columns, units, null);
}
public void addNumericField(String label, double defaultValue, int digits, int columns, String units, String tooltip) {
String svalue = IJ.d2s(defaultValue, digits); // , columns
JTextField inp = new JTextField();
inp.setText(svalue);
inp.setColumns(columns/DIVIDE_COLUMNS_BY + 2);
JPanel inp_units = new JPanel(false);
inp_units.add(inp);
if ((units != null) && !units.isEmpty()) {
JLabel junits = new JLabel(units);
Font fnt = junits.getFont();
junits.setFont(fnt.deriveFont(fnt.getStyle() | Font.ITALIC));
inp_units.add(junits);
}
inp_units.putClientProperty("type", "double");
inp_units.setLayout(new FlowLayout(FlowLayout.LEFT));
addLine(label, inp_units, tooltip);
// Component[] comps = inp_units.getComponents();
}
public void addCheckbox(String label, boolean defaultValue) {
addCheckbox(label, defaultValue, null);
}
public void addCheckbox(String label, boolean defaultValue, String tooltip) { // no support for preview functionality
JCheckBox checkbox = new JCheckBox(null, null, defaultValue); // text, icon, selected
checkbox.putClientProperty("type", "boolean");
addLine(label, checkbox, tooltip);
}
// Add choice too.
public void addChoice(String label, String[] items, String defaultItem) {
addChoice(label, items, defaultItem, null);
}
public void addChoice(String label, String[] items, String defaultItem, String tooltip) {
int index = 0;
if (defaultItem != null) {
for (int i = 0; i < items.length; i++) if (items[i].equals(defaultItem)) {
index = i;
break;
}
}
JComboBox<String> combo = new JComboBox<String>(items);
combo.setSelectedIndex(index);
JPanel inp_units = new JPanel(false);
inp_units.add(combo);
inp_units.putClientProperty("type", "combo");
// combo.setPreferredSize(new Dimension(200));
// combo.setSize(200, combo.getPreferredSize().height);
// combo.setMaximumSize(20); // combo.getPreferredSize() );
inp_units.setLayout(new FlowLayout(FlowLayout.LEFT));
addLine(label, inp_units, tooltip);
}
public void buildDialog() { // non-blocking, does not show
if (buttons.isEmpty()) {
addDefaultButtons();
}
jd.setLayout(new BorderLayout()); // new GridLayout(2, 2) );
// prepare each panel first, then optionally make a tabbed layout
for (int ntab = 0; ntab < tabs.size(); ntab++) {
JComponent tab = tabs.get(ntab);
ArrayList<JComponent> tabComponents = components.get(ntab);
ArrayList<JLabel> tabLabels = labels.get(ntab);
tab.setLayout(new GridBagLayout());
GridBagConstraints gbc = new GridBagConstraints();
gbc.fill = GridBagConstraints.HORIZONTAL;
gbc.weightx = 0.5;
gbc.weighty = 0.0;
for (int ncomp = 0; ncomp < tabLabels.size(); ncomp++) {
JLabel label = tabLabels.get(ncomp);
JComponent component = tabComponents.get(ncomp);
gbc.gridx = 0;
gbc.gridy = ncomp;
gbc.gridwidth = 1;
gbc.ipady = 0;
if (component == null) { // message/comment line
label.setHorizontalAlignment(JLabel.CENTER);
label.setForeground(COMMENT_COLOR);
Font fnt = label.getFont();
label.setFont(fnt.deriveFont(fnt.getStyle() | Font.ITALIC));
gbc.gridwidth = 2;
gbc.ipady = COMMENT_PADY;
tab.add(label,gbc);
} else {
label.setHorizontalAlignment(JLabel.RIGHT);
tab.add(label,gbc);
gbc.gridx = 1;
tab.add(component,gbc);
}
}
// Add empty label that would push up all other rows
JLabel label_push = new JLabel("");
gbc.weightx = 0.5;
gbc.weighty = 0.5;
gbc.gridx = 0;
gbc.gridy = tabLabels.size();
tab.add(label_push,gbc);
// Wrap each panel with scroll panel
JScrollPane scrollPane = new JScrollPane(tab);
scrollPane.setHorizontalScrollBarPolicy(JScrollPane.HORIZONTAL_SCROLLBAR_AS_NEEDED); // HORIZONTAL_SCROLLBAR_NEVER);
scrollPane.setVerticalScrollBarPolicy(JScrollPane.VERTICAL_SCROLLBAR_AS_NEEDED);
scrollPanes.add(scrollPane);
}
Border padding = BorderFactory.createEmptyBorder(20,20,5,20);
if (tabs.size() == 1) {
jd.add(new JScrollPane(scrollPanes.get(0)), BorderLayout.CENTER); // no tabs
} else { // tabbed
JTabbedPane tabbedPane = new JTabbedPane();
for (int ntab = 0; ntab < tabs.size(); ntab++) {
JScrollPane scrollPane = scrollPanes.get(ntab);
JComponent tab = tabs.get(ntab);
scrollPane.setBorder(padding);
tabbedPane.addTab(
(String) tab.getClientProperty("title"),
null, // icon
scrollPane,
(String) tab.getClientProperty("tooltip"));
}
jd.add(tabbedPane, BorderLayout.CENTER); //
}
// Add buttons
JPanel panelButtons = new JPanel(false);
panelButtons.setLayout(new FlowLayout());
for (JButton b : buttons) {
panelButtons.add(b);
b.addActionListener(this);
}
jd.add(panelButtons, BorderLayout.PAGE_END);
jd.setSize(width,height);
}
public String showDialogAny() {
buildDialog();
jd.setVisible(true);
return result;
}
public boolean showDialog() {
buildDialog();
jd.setVisible(true);
if ((result != null) && !result.equals("Cancel")) return true;
return false;
}
@Override
public void actionPerformed(ActionEvent e) {
System.out.println(e.getActionCommand());
if (e.getActionCommand().equals("Cancel")) {
result = e.getActionCommand();
jd.dispose();
} else if (e.getActionCommand().equals("OK")) {
result = e.getActionCommand();
jd.dispose();
}
}
public boolean wasCanceled() {
return (result == null) || (result.equals("Cancel"));
}
private boolean skipComments (
boolean incTab) { // increment tab if nothing left in this one (false - keep read_component equal to tab's components length
boolean got_it = false;
// boolean nothing_left = false;
// read_component ++;
while (!got_it) {
if (read_component >= labels.get(read_tab).size()) {
if (!incTab || (read_tab >= labels.size())) break;
read_tab++;
read_component = 0;
}
if (components.get(read_tab).get(read_component) != null) {
got_it = true;
} else {
read_component++;
}
}
return got_it;
}
public void checkType (String type) { // type == null for null components OK
JComponent component = components.get(read_tab).get(read_component);
String this_type = "null";
if (component != null) {
this_type = (String) component.getClientProperty("type");
if (this_type.equals(type)) return;
} else if (type == null) return;
// component type mismatch - report
String msg="Dialog components mismatch for tab="+read_tab+", component="+read_component+
", label='"+labels.get(read_tab).get(read_component).getText()+"': expected "+type+", got "+this_type;
IJ.showMessage(msg);
throw new IllegalArgumentException (msg);
}
public boolean getNextBoolean() {
skipComments(true);
checkType("boolean");
boolean rslt = ((JCheckBox) components.get(read_tab).get(read_component)).isSelected();
read_component ++;
return rslt;
}
public String getNextString() {
skipComments(true); // add testing for checkbox?
checkType("String");
String rslt = ((JTextField) components.get(read_tab).get(read_component).getComponents()[0]).getText();
read_component ++;
return rslt;
}
public double getNextNumber() {
skipComments(true); // add testing for checkbox?
checkType("double");
double rslt = Double.parseDouble(((JTextField) components.get(read_tab).get(read_component).getComponents()[0]).getText());
read_component ++;
return rslt;
}
public int getNextChoiceIndex() {
skipComments(true); // add testing for checkbox?
checkType("combo");
@SuppressWarnings("unchecked")
JComboBox<String> combo = (JComboBox<String>) components.get(read_tab).get(read_component).getComponents()[0];
String selectedItem = (String)combo.getSelectedItem();
// Object [] items = combo.getSelectedObjects();
int size = combo.getItemCount();
int index = 0;
if (selectedItem != null) {
for (int i = 0; i < size; i++) if (combo.getItemAt(i).equals(selectedItem)) {
index = i;
break;
}
}
System.out.println(combo.getSelectedItem());
// for (int i = 0; i < selectedItem.length; i++){
// System.out.println(String.format("item %s = %s", i, selectedItem[i]));
// }
read_component ++;
return index;
}
/*
String[] selectedItem = (String[])combo.getSelectedItem();
for (int i = 0; i < selectedItem.length; i++){
System.out.println(String.format("item %s = %s", i, selectedItem[i]));
}
*/
}
\ No newline at end of file
src/main/java/GeometryCorrection.java
View file @ 3f7a67de
...@@ -9,7 +9,7 @@ import ij.IJ; ...@@ -9,7 +9,7 @@ import ij.IJ;
** Copyright (C) 2017 Elphel, Inc. ** Copyright (C) 2017 Elphel, Inc.
** **
** -----------------------------------------------------------------------------** ** -----------------------------------------------------------------------------**
** **
** GeometryCorrection.java is free software: you can redistribute it and/or modify ** GeometryCorrection.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by ** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or ** the Free Software Foundation, either version 3 of the License, or
...@@ -29,15 +29,9 @@ import ij.IJ; ...@@ -29,15 +29,9 @@ import ij.IJ;
public class GeometryCorrection { public class GeometryCorrection {
static double SCENE_UNITS_SCALE = 0.001; static double SCENE_UNITS_SCALE = 0.001;
static String SCENE_UNITS_NAME = "m"; static String SCENE_UNITS_NAME = "m";
static final String [] CORR_NAMES = {"tilt0","tilt1","tilt2","azimuth0","azimuth1","azimuth2","roll0","roll1","roll2","roll3"}; static final String [] CORR_NAMES = {"tilt0","tilt1","tilt2","azimuth0","azimuth1","azimuth2","roll0","roll1","roll2","roll3","zoom0","zoom1","zoom2"};
public int debugLevel = 0; public int debugLevel = 0;
// public double azimuth; // azimuth of the lens entrance pupil center, degrees, clockwise looking from top
// public double radius; // mm, distance from the rotation axis
// public double height; // mm, up - from the origin point
// public double phi; // degrees, optical axis from azimuth/r vector, clockwise heading
// public double theta; // degrees, optical axis from the eyesis horizon, positive - up elevation
// public double psi; // degrees, rotation (of the sensor) around the optical axis. Positive if camera is rotated clockwise looking to the target roll
public int pixelCorrectionWidth=2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2) public int pixelCorrectionWidth=2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2)
public int pixelCorrectionHeight=1936; public int pixelCorrectionHeight=1936;
public double focalLength=4.5; public double focalLength=4.5;
...@@ -50,31 +44,26 @@ public class GeometryCorrection { ...@@ -50,31 +44,26 @@ public class GeometryCorrection {
public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?) public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
public double distortionB=0.0; // r^3 public double distortionB=0.0; // r^3
public double distortionC=0.0; // r^2 public double distortionC=0.0; // r^2
// public double px0=1296.0; // center of the lens on the sensor, pixels
// public double py0=968.0; // center of the lens on the sensor, pixels
// parameters, common for all sensors // parameters, common for all sensors
public double elevation = 0.0; // degrees, up - positive; public double elevation = 0.0; // degrees, up - positive;
public double heading = 0.0; // degrees, CW (from top) - positive public double heading = 0.0; // degrees, CW (from top) - positive
// public double roll_common = 0.0; // degrees, CW (to target) - positive
public int numSensors = 4; public int numSensors = 4;
private double [] forward = null; private double [] forward = null;
private double [] right = null; private double [] right = null;
private double [] height = null; private double [] height = null;
private double [] roll = null; // degrees, CW (to target) - positive private double [] roll = null; // degrees, CW (to target) - positive
public double [][] pXY0 = null; // sensor center XY in pixels public double [][] pXY0 = null; // sensor center XY in pixels
private double common_right; // mm right, camera center private double common_right; // mm right, camera center
private double common_forward; // mm forward (to target), camera center private double common_forward; // mm forward (to target), camera center
private double common_height; // mm up, camera center private double common_height; // mm up, camera center
private double common_roll; // degrees CW (to target) camera as a whole private double common_roll; // degrees CW (to target) camera as a whole
private double [][] XYZ_he; // all cameras coordinates transformed to eliminate heading and elevation (rolls preserved) private double [][] XYZ_he; // all cameras coordinates transformed to eliminate heading and elevation (rolls preserved)
private double [][] XYZ_her = null; // XYZ of the lenses in a corrected CCS (adjusted for to elevation, heading, common_roll) private double [][] XYZ_her = null; // XYZ of the lenses in a corrected CCS (adjusted for to elevation, heading, common_roll)
private double [][] rXY = null; // XY pairs of the in a normal plane, relative to disparityRadius private double [][] rXY = null; // XY pairs of the in a normal plane, relative to disparityRadius
private double [][] rXY_ideal = {{-0.5, -0.5}, {0.5,-0.5}, {-0.5, 0.5}, {0.5,0.5}}; private double [][] rXY_ideal = {{-0.5, -0.5}, {0.5,-0.5}, {-0.5, 0.5}, {0.5,0.5}};
public double cameraRadius=0; // average distance from the "mass center" of the sensors to the sensors public double cameraRadius=0; // average distance from the "mass center" of the sensors to the sensors
public double disparityRadius=0; // distance between cameras to normalize disparity units to. sqrt(2)*disparityRadius for quad camera (~=150mm)? public double disparityRadius=0; // distance between cameras to normalize disparity units to. sqrt(2)*disparityRadius for quad camera (~=150mm)?
...@@ -89,7 +78,7 @@ public class GeometryCorrection { ...@@ -89,7 +78,7 @@ public class GeometryCorrection {
{ {
this.extrinsic_corr = new CorrVector(extrinsic_corr); this.extrinsic_corr = new CorrVector(extrinsic_corr);
} }
// correction of cameras mis-alignment // correction of cameras mis-alignment
public CorrVector getCorrVector(double [] vector){ public CorrVector getCorrVector(double [] vector){
return new CorrVector(vector); return new CorrVector(vector);
...@@ -113,54 +102,44 @@ public class GeometryCorrection { ...@@ -113,54 +102,44 @@ public class GeometryCorrection {
public boolean [] getParMask( public boolean [] getParMask(
boolean use_disparity, boolean use_disparity,
boolean common_roll) boolean common_roll,
boolean corr_focalLength)
{ {
return (new CorrVector()).getParMask( return (new CorrVector()).getParMask(
use_disparity, use_disparity,
common_roll); common_roll,
corr_focalLength);
} }
public class CorrVector{ public class CorrVector{
static final int LENGTH = 10; static final int LENGTH = 13; // 10;
static final int LENGTH_ANGLES =10;
static final int TILT_INDEX = 0; static final int TILT_INDEX = 0;
static final int AZIMUTH_INDEX = 3; static final int AZIMUTH_INDEX = 3;
static final int ROLL_INDEX = 6; static final int ROLL_INDEX = 6;
static final int ZOOM_INDEX = 10;
static final double ROT_AZ_SGN = -1.0; // sign of first sin for azimuth rotation 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_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 static final double ROT_RL_SGN = 1.0; // sign of first sin for roll rotation
double [] vector; double [] vector;
public Matrix [] getRotMatrices() public Matrix [] getRotMatrices()
{ {
Matrix [] rots = new Matrix [4]; Matrix [] rots = new Matrix [4];
double [] azimuths = getAzimuths(); double [] azimuths = getAzimuths();
double [] tilts = getTilts(); double [] tilts = getTilts();
double [] rolls = getFullRolls(); double [] rolls = getFullRolls();
double [] zooms = getZooms();
for (int chn = 0; chn < rots.length; chn++) { for (int chn = 0; chn < rots.length; chn++) {
double ca = Math.cos(azimuths[chn]); double ca = Math.cos(azimuths[chn]);
double sa = Math.sin(azimuths[chn]); double sa = Math.sin(azimuths[chn]);
double ct = Math.cos(tilts[chn]); double ct = Math.cos(tilts[chn]);
double st = Math.sin(tilts[chn]); double st = Math.sin(tilts[chn]);
double cr = Math.cos(rolls[chn]); double zoom = (1.0 + zooms[chn]);
double sr = Math.sin(rolls[chn]); double cr = Math.cos(rolls[chn]) * zoom;
/* double sr = Math.sin(rolls[chn]) * zoom;
double [][] a_az = { // inverted - OK
{ ca, 0.0, -sa},
{ 0.0, 1.0, 0.0},
{ sa, 0.0, ca}};
double [][] a_t = { // inverted - OK
{ 1.0, 0.0, 0.0},
{ 0.0, ct, st},
{ 0.0, -st, ct}};
double [][] a_r = { // inverted OK
{ cr, sr, 0.0},
{ -sr, cr, 0.0},
{ 0.0, 0.0, 1.0}};
*/
double [][] a_az = { // inverted - OK double [][] a_az = { // inverted - OK
{ ca, 0.0, sa * ROT_AZ_SGN }, { ca, 0.0, sa * ROT_AZ_SGN },
{ 0.0, 1.0, 0.0}, { 0.0, 1.0, 0.0},
...@@ -181,22 +160,25 @@ public class GeometryCorrection { ...@@ -181,22 +160,25 @@ public class GeometryCorrection {
return rots; return rots;
} }
/** /**
* Get derivatives of the rotation matrices, per sensor per axis (azimuth, tilt, roll) * Get derivatives of the rotation matrices, per sensor per axis (azimuth, tilt, roll, zoom)
* d/dx and d/dy should be normalized by z-component of the vector (not derivative) * d/dx and d/dy should be normalized by z-component of the vector (not derivative)
* @return 2-d array array of derivatives matrices * @return 2-d array array of derivatives matrices
*/ */
//TODO: UPDATE to include scales
public Matrix [][] getRotDeriveMatrices() public Matrix [][] getRotDeriveMatrices()
{ {
Matrix [][] rot_derivs = new Matrix [4][3]; // channel, azimuth-tilt-roll Matrix [][] rot_derivs = new Matrix [4][4]; // channel, azimuth-tilt-roll-zoom
double [] azimuths = getAzimuths(); double [] azimuths = getAzimuths();
double [] tilts = getTilts(); double [] tilts = getTilts();
double [] rolls = getFullRolls(); double [] rolls = getFullRolls();
double [] zooms = getZooms();
for (int chn = 0; chn < rot_derivs.length; chn++) { for (int chn = 0; chn < rot_derivs.length; chn++) {
double ca = Math.cos(azimuths[chn]); double ca = Math.cos(azimuths[chn]);
double sa = Math.sin(azimuths[chn]); double sa = Math.sin(azimuths[chn]);
double ct = Math.cos(tilts[chn]); double ct = Math.cos(tilts[chn]);
double st = Math.sin(tilts[chn]); double st = Math.sin(tilts[chn]);
double zoom = (1.0 + zooms[chn]);
double cr = Math.cos(rolls[chn]); double cr = Math.cos(rolls[chn]);
double sr = Math.sin(rolls[chn]); double sr = Math.sin(rolls[chn]);
double [][] a_az = { // inverted - OK double [][] a_az = { // inverted - OK
...@@ -213,24 +195,7 @@ public class GeometryCorrection { ...@@ -213,24 +195,7 @@ public class GeometryCorrection {
{ cr, sr * ROT_RL_SGN, 0.0}, { cr, sr * ROT_RL_SGN, 0.0},
{ -sr * ROT_RL_SGN, cr, 0.0}, { -sr * ROT_RL_SGN, cr, 0.0},
{ 0.0, 0.0, 1.0}}; { 0.0, 0.0, 1.0}};
/*
double [][] a_daz = { // inverted - OK
{ -sa, 0.0, ca * ROT_AZ_SGN },
{ 0.0, 1.0, 0.0},
{ -ca* ROT_AZ_SGN, 0.0, -sa}};
double [][] a_dt = { // inverted - OK
{ 1.0, 0.0, 0.0},
{ 0.0, -st, ct * ROT_TL_SGN},
{ 0.0, -ct * ROT_TL_SGN, -st}};
double [][] a_dr = { // inverted OK
{ -sr, cr * ROT_RL_SGN, 0.0},
{ -cr * ROT_RL_SGN, -sr, 0.0},
{ 0.0, 0.0, 1.0}};
*/
double [][] a_daz = { // inverted - OK double [][] a_daz = { // inverted - OK
{ -sa, 0.0, ca * ROT_AZ_SGN }, { -sa, 0.0, ca * ROT_AZ_SGN },
{ 0.0, 0.0, 0.0}, { 0.0, 0.0, 0.0},
...@@ -242,25 +207,31 @@ public class GeometryCorrection { ...@@ -242,25 +207,31 @@ public class GeometryCorrection {
{ 0.0, -ct * ROT_TL_SGN, -st}}; { 0.0, -ct * ROT_TL_SGN, -st}};
double [][] a_dr = { // inverted OK double [][] a_dr = { // inverted OK
{ -sr, cr * ROT_RL_SGN, 0.0}, { -sr * zoom, cr * zoom * ROT_RL_SGN, 0.0},
{ -cr * ROT_RL_SGN, -sr, 0.0}, { -cr * zoom *ROT_RL_SGN, -sr * zoom, 0.0},
{ 0.0, 0.0, 0.0}}; { 0.0, 0.0, 0.0}};
double [][] a_dzoom = { // inverted OK
{ cr, sr * ROT_RL_SGN, 0.0},
{ -sr * ROT_RL_SGN, cr, 0.0},
{ 0.0, 0.0, 0.0}};
// d/d_az // d/d_az
rot_derivs[chn][0] = (new Matrix(a_r ).times(new Matrix(a_t ).times(new Matrix(a_daz)))); rot_derivs[chn][0] = (new Matrix(a_r ).times(new Matrix(a_t ).times(new Matrix(a_daz))));
rot_derivs[chn][1] = (new Matrix(a_r ).times(new Matrix(a_dt).times(new Matrix(a_az )))); rot_derivs[chn][1] = (new Matrix(a_r ).times(new Matrix(a_dt).times(new Matrix(a_az ))));
rot_derivs[chn][2] = (new Matrix(a_dr).times(new Matrix(a_t ).times(new Matrix(a_az )))); rot_derivs[chn][2] = (new Matrix(a_dr).times(new Matrix(a_t ).times(new Matrix(a_az ))));
rot_derivs[chn][3] = (new Matrix(a_dzoom).times(new Matrix(a_t ).times(new Matrix(a_az ))));
} }
return rot_derivs; return rot_derivs;
} }
public CorrVector () public CorrVector ()
{ {
this.vector = new double[10]; this.vector = new double[LENGTH];
} }
public CorrVector ( public CorrVector (
...@@ -270,23 +241,29 @@ public class GeometryCorrection { ...@@ -270,23 +241,29 @@ public class GeometryCorrection {
this.vector = toTarArray(sym_vector, par_mask); this.vector = toTarArray(sym_vector, par_mask);
} }
public CorrVector ( public CorrVector (
double tilt0, double tilt1, double tilt2, double tilt0, double tilt1, double tilt2,
double azimuth0, double azimuth1, double azimuth2, double azimuth0, double azimuth1, double azimuth2,
double roll0, double roll1, double roll2, double roll3) double roll0, double roll1, double roll2, double roll3,
double zoom0, double zoom1, double zoom2)
{ {
double [] v = { double [] v = {
tilt0, tilt1, tilt2, tilt0, tilt1, tilt2,
azimuth0, azimuth1, azimuth2, azimuth0, azimuth1, azimuth2,
roll0, roll1, roll2, roll3}; roll0, roll1, roll2, roll3,
zoom0, zoom1, zoom2};
this.vector = v; this.vector = v;
} }
public CorrVector (double [] vector) public CorrVector (double [] vector)
{ {
if (vector != null) { if (vector != null) {
if (vector.length != LENGTH) {
throw new IllegalArgumentException("vector.length = "+vector.length+" != "+LENGTH);
}
this.vector = vector; this.vector = vector;
} }
} }
...@@ -295,22 +272,24 @@ public class GeometryCorrection { ...@@ -295,22 +272,24 @@ public class GeometryCorrection {
* @param tilt for subcameras 0..2, radians, positive - up (subcamera 3 so sum == 0) * @param tilt for subcameras 0..2, radians, positive - up (subcamera 3 so sum == 0)
* @param azimuth for subcameras 0..2, radians, positive - right (subcamera 3 so sum == 0) * @param azimuth for subcameras 0..2, radians, positive - right (subcamera 3 so sum == 0)
* @param roll for subcameras 0..3, radians, positive - CW looking to the target * @param roll for subcameras 0..3, radians, positive - CW looking to the target
* @param zoom for subcameras 0..2, difference from 1.0 . Positive - image is too small, needs to be zoomed in by (1.0 + scale)
*/ */
public CorrVector (double [] tilt, double [] azimuth, double [] roll) public CorrVector (double [] tilt, double [] azimuth, double [] roll, double [] zoom)
{ {
double [] vector = { double [] vector = {
tilt[0], tilt[1], tilt[2], tilt[0], tilt[1], tilt[2],
azimuth[0], azimuth[1], azimuth[2], azimuth[0], azimuth[1], azimuth[2],
roll[0], roll[1], roll[2], roll[3]}; roll[0], roll[1], roll[2], roll[3],
this.vector = vector; zoom[0], zoom[1], zoom[2]};
this.vector = vector;
} }
public CorrVector getCorrVector(double [] vector){ public CorrVector getCorrVector(double [] vector){
return new CorrVector(vector); return new CorrVector(vector);
} }
public double [] toArray() public double [] toArray()
{ {
return vector; return vector;
...@@ -346,6 +325,18 @@ public class GeometryCorrection { ...@@ -346,6 +325,18 @@ public class GeometryCorrection {
return vector[6 + indx]; return vector[6 + indx];
} }
public double [] getZooms()
{
double [] zooms = {vector[10], vector[11], vector[12], - (vector[10] + vector[11] +vector[12])};
return zooms;
}
public double getZoom(int indx)
{
if (indx == 3) return - (vector[10] + vector[11] +vector[12]);
else return vector[10 + indx];
}
// Include factory calibration rolls // Include factory calibration rolls
public double [] getFullRolls() public double [] getFullRolls()
{ {
...@@ -362,30 +353,34 @@ public class GeometryCorrection { ...@@ -362,30 +353,34 @@ public class GeometryCorrection {
return vector[6 + indx] + roll[indx] * Math.PI/180.0; return vector[6 + indx] + roll[indx] * Math.PI/180.0;
} }
@Override
public String toString() public String toString()
{ {
String s; String s;
double [] sym_vect = toSymArray(null); double [] sym_vect = toSymArray(null);
double [] v = new double [vector.length]; double [] v = new double [vector.length];
double [] sv = new double [vector.length]; double [] sv = new double [vector.length];
for (int i = 0; i < vector.length; i++){ for (int i = 0; i < LENGTH; i++){
v[i] = vector[i]*180/Math.PI; if (i < LENGTH_ANGLES) {
sv[i] = sym_vect[i]*180/Math.PI; v[i] = vector[i]*180/Math.PI;
sv[i] = sym_vect[i]*180/Math.PI;
} else {
v[i] = vector[i];
sv[i] = sym_vect[i];
}
} }
s = String.format("tilt (up): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) ); s = String.format("tilt (up): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) );
s += String.format("azimuth (right): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) ); s += String.format("azimuth (right): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) );
s += String.format("roll (CW): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[6], v[7], v[8], v[9] ); s += String.format("roll (CW): %8.5f° %8.5f° %8.5f° %8.5f°\n" , v[6], v[7], v[8], v[9] );
s += String.format("diff zoom (in): %8.5f‰ %8.5f‰ %8.5f‰ %8.5f‰\n" , 1000*v[10],1000*v[11],1000*v[12], -1000*(v[10] + v[11] + v[12]) );
s += "Symmetrical vector:\n"; s += "Symmetrical vector:\n";
s += " |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘| 6: common roll 7:(r0-r3)/2, \n"; s += " |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘| 6: common roll 7:(r0-r3)/2, |- +| |- -| |- +|\n";
s += "0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖| 8:(r1-r2)/2 9:(r0+r3-r1-r2)/4\n"; s += " 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖| 8:(r1-r2)/2 9:(r0+r3-r1-r2)/4 10: |- +| 11: |+ +| 12: |+ -|\n";
s += String.format("0: %8.5f° 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°\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9] ); s += String.format(" 0: %8.5f° 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" ,
// s += String.format("sym_vect: 0: %10.7f 1:%10.7f 2:%10.7f 3:%10.7f 4:%10.7f 5:%10.7f 6:%10.7f 7:%10.7f 8:%10.7f 9:%10.7f\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] );
// sym_vect[0], sym_vect[1], sym_vect[2], sym_vect[3], sym_vect[4], sym_vect[5], sym_vect[6], sym_vect[7], sym_vect[8], sym_vect[9] );
// s += String.format("vector: 0: %10.7f 1:%10.7f 2:%10.7f 3:%10.7f 4:%10.7f 5:%10.7f 6:%10.7f 7:%10.7f 8:%10.7f 9:%10.7f\n" ,
// vector[0], vector[1], vector[2], vector[3], vector[4], vector[5], vector[6], vector[7], vector[8], vector[9] );
return s; return s;
} }
public void incrementVector(double [] incr, public void incrementVector(double [] incr,
...@@ -395,19 +390,20 @@ public class GeometryCorrection { ...@@ -395,19 +390,20 @@ public class GeometryCorrection {
vector[i]+= incr[i] * scale; vector[i]+= incr[i] * scale;
} }
} }
public void incrementVector(CorrVector incr, double scale) public void incrementVector(CorrVector incr, double scale)
{ {
incrementVector(incr.toArray(), scale); incrementVector(incr.toArray(), scale);
} }
@Override
public CorrVector clone(){ public CorrVector clone(){
return new CorrVector(this.vector.clone()); return new CorrVector(this.vector.clone());
} }
/** /**
* Convert manual pixel shift between images to azimuth/tilt rotations (distortions ignored) * Convert manual pixel shift between images to azimuth/tilt rotations (distortions ignored)
* and apply (add) them to the current vector (normally should be all 0.0) * and apply (add) them to the current vector (normally should be all 0.0)
* @param pXY_shift manula XY pixel corrections (shiftXY made of clt_parameters.fine_corr_[xy]_[0123]) * @param pXY_shift manula XY pixel corrections (shiftXY made of clt_parameters.fine_corr_[xy]_[0123])
*/ */
public void applyPixelShift(double [][] pXY_shift){ public void applyPixelShift(double [][] pXY_shift){
...@@ -423,35 +419,35 @@ public class GeometryCorrection { ...@@ -423,35 +419,35 @@ public class GeometryCorrection {
vector[AZIMUTH_INDEX + i] += pix_to_rads * (pXY_shift[i][0] - pXY_avg[0]); vector[AZIMUTH_INDEX + i] += pix_to_rads * (pXY_shift[i][0] - pXY_avg[0]);
} }
} }
/** /**
* Get conversion matrix from symmetrical coordinates * Get conversion matrix from symmetrical coordinates
* @return * @return
* *
* |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘| * |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘|
* 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖| * 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖|
* *
*/ */
public double [][] dSym_j_dTar_i() public double [][] dSym_j_dTar_i()
{ {
//Previous does not consider movement of the 4-th sensor, so t3 = -t0+t1+t2), a3 = -(a0+a1+a2)
double [][] tar_to_sym = { double [][] tar_to_sym = {
{-2.0, -2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // t0 {-2.0, -2.0, 2.0, -2.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}, // t1 {-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}, // t1
{ 0.0, -2.0, 2.0, 0.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0}, // t2 { 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}, // t2
{ 2.0, 2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, // a0 { 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}, // a0
{ 0.0, 2.0, 2.0, 0.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0}, // a1 { 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}, // a1
{ 2.0, 0.0, 0.0, -2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0}, // a2 { 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}, // a2
// { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0}, // roll 0 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.5, 0.0, 0.25, 0.0, 0.0, 0.0}, // roll 0
// { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, // roll 1 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.5, -0.25, 0.0, 0.0, 0.0}, // roll 1
// { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, // roll 2 { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, -0.5, -0.25, 0.0, 0.0, 0.0}, // roll 2
// { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}};// roll 3 { 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}, // roll 3
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.5, 0.0, 0.25}, // roll 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.5, -0.25}, // roll 1 { 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}, // scale 0
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, -0.5, -0.25}, // roll 2 { 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}, // scale 1
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, -0.5, 0.0, 0.25}};// roll 3 { 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} // scale 2
};
return tar_to_sym; return tar_to_sym;
} }
...@@ -460,119 +456,103 @@ public class GeometryCorrection { ...@@ -460,119 +456,103 @@ public class GeometryCorrection {
// Matrix tar_to_sym = sym_to_tar.inverse(); // Matrix tar_to_sym = sym_to_tar.inverse();
// return tar_to_sym.getArray(); // return tar_to_sym.getArray();
/* /*
>>> a=[[-2, -2, 2, -2, 0, 0, 0, 0, 0, 0], [-2, 0, 0, -2, 2, -2, 0, 0, 0, 0], [0, -2, 2, 0, 2, -2, 0, 0, 0, 0], [2, 2, 2, -2, 0, 0, 0, 0, 0, 0], [0, 2, 2, 0, 2, 2, 0, 0, 0, 0], a=[
... [2, 0, 0, -2, 2, 2, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1]] [-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],
>>> m = numpy.matrix(a) [-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],
>>> m [ 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],
matrix([[-2, -2, 2, -2, 0, 0, 0, 0, 0, 0], [ 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],
[-2, 0, 0, -2, 2, -2, 0, 0, 0, 0], [ 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, -2, 2, 0, 2, -2, 0, 0, 0, 0], [ 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],
[ 2, 2, 2, -2, 0, 0, 0, 0, 0, 0], [ 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, 2, 2, 0, 2, 2, 0, 0, 0, 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],
[ 2, 0, 0, -2, 2, 2, 0, 0, 0, 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, 1, 0, 0, 0], [ 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, 1, 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, 1, 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, 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]]
>>> numpy.linalg.inv(m)
matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, 0. , -0. , -0. , -0. ],
[-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ], matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. , -0. , -0. , -0. , -0. ],
[ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ], [-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. , 0. , 0. , 0. ],
[-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ], [ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. , 0. , 0. , 0. ],
[-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ], [-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0. , 0. , 0. , 0. , 0. , 0. , 0. ],
[ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ], [-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. , 0. , 0. ], [ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. , 0. ], [ 0. , 0. , 0. , 0. , 0. , 0. , 1. , 1. , 1. , 1. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. , 0. ], [-0. , -0. , -0. , -0. , -0. , -0. , 1. , -0. , -0. , -1. , -0. , -0. , -0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1. ]]) [-0. , -0. , -0. , -0. , -0. , -0. , -0. , 1. , -1. , -0. , -0. , -0. , -0. ],
[-0. , -0. , -0. , -0. , -0. , -0. , 1. , -1. , -1. , 1. , -0. , -0. , -0. ],
[-0. , -0. , -0. , -0. , -0. , -0. , -0. , -0. , -0. , -0. , -0.5 , 0.5 , -0.5 ],
a=[[-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.5 , -0.5 , 0.5 ],
[-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.5 , 0.5 , 0.5 ]])
[ 0.0, -2.0, 2.0, 0.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0], */
[ 2.0, 2.0, 2.0, -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 2.0, 2.0, 0.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0],
[ 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.25, 0.5, 0.0, 0.25],
[ 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.25, 0.0, -0.5, -0.25],
[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, -0.5, 0.0, 0.25]]
matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. , -0. ],
[-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0. , 0. , 0. , 0. ],
[-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ],
[ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. , 0. , 0. , 1. , 1. , 1. , 1. ],
[-0. , -0. , -0. , -0. , -0. , -0. , 1. , -0. , -0. , -1. ],
[-0. , -0. , -0. , -0. , -0. , -0. , -0. , 1. , -1. , -0. ],
[-0. , -0. , -0. , -0. , -0. , -0. , 1. , -1. , -1. , 1. ]])
*/
double [][] sym_to_tar= { double [][] sym_to_tar= {
// t0 t1 t2 a0 a1 a2 r0 r1 r2 r3 // 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 }, // 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 }, // sym0
{-0.125, 0.125, -0.125, 0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym1 {-0.125, 0.125,-0.125, 0.125, 0.125,-0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym1
{ 0.125, -0.125, 0.125, 0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym2 { 0.125,-0.125, 0.125, 0.125, 0.125,-0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym2
{-0.125, -0.125, 0.125, -0.125, 0.125, -0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym3 {-0.125,-0.125, 0.125,-0.125, 0.125,-0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym3
{-0.125, 0.125, 0.125, -0.125, 0.125, 0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym4 {-0.125, 0.125, 0.125,-0.125, 0.125, 0.125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // sym4
{ 0.125, -0.125, -0.125, -0.125, 0.125, 0.125, 0.0 , 0.0 , 0.0 , 0.0 }, // sym5 { 0.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 }, // sym5
// { 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , 0.0 }, // sym6 = r0 { 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0, 1.0, 1.0, 1.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 , 0.0 , 1.0 , 0.0 , 0.0 }, // sym7 = r1 { 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 }, // sym7 = (r0-r3)/2
// { 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 }, // sym8 = r2 { 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 1.0, -1.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 , 0.0 , 0.0 , 0.0 , 1.0 }}; // sym9 = r3 { 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 }, // sym9 = (r0+r3-r1-r2)/4
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 1.0 , 1.0 , 1.0 }, // sym6 = (r0+r1+r2+r3)/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 }, // sym10 = -s0 - s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , 0.0 , 0.0 , -1.0 }, // sym7 = (r0-r3)/2 { 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 }, // sym11 = -s0 - s1
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , -1.0 , 0.0 }, // sym8 = (r1-r2)/2 { 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 } // sym12 = s1 + s2
{ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 1.0 , -1.0 , -1.0 , 1.0 }}; // sym9 = (r0+r3-r1-r2)/4 };
return sym_to_tar; return sym_to_tar;
} }
public boolean [] getParMask( public boolean [] getParMask(
boolean use_disparity, boolean use_disparity,
boolean common_roll) boolean common_roll,
boolean corr_focalLength)
{ {
boolean [] par_mask = { // TODO: move to GeometryCorrection boolean [] par_mask = {
use_disparity, //sym0 use_disparity, //sym0
true, //sym1 true, //sym1
true, //sym2 true, //sym2
true, //sym3 true, //sym3
true, //sym4 true, //sym4
true, //sym5 true, //sym5
common_roll, //sym6 // common roll common_roll, //sym6 // common roll
true, //sym7 true, //sym7
true, //sym8 true, //sym8
true //sym9 true, //sym9
corr_focalLength, //sym10
corr_focalLength, //sym11
corr_focalLength //sym12
}; };
return par_mask; return par_mask;
} }
/** /**
* Get partial transposed Jacobian as 2d array (for one measurement set) from partial Jacobian for each sample * Get partial transposed Jacobian as 2d array (for one measurement set) from partial Jacobian for each sample
* with derivatives of port coordinates (all 4) by 3 tilts (ports 0..2), 3 azimuths (ports 0..2) and all 4 rolls * with derivatives of port coordinates (all 4) by 3 tilts (ports 0..2), 3 azimuths (ports 0..2) and all 4 rolls
* Tilt and azimuth for port 3 is calculated so center would not move. Tilt is positive up, azimuth - right and * Tilt and azimuth for port 3 is calculated so center would not move. Tilt is positive up, azimuth - right and
* roll - clockwise * roll - clockwise
* *
* Result is transposed Jacobian (rows (9 or 10) - parameters, columns - port coordinate components (8). Parameters * Result is transposed Jacobian (rows (9 , 10,12 or 13) - parameters, columns - port coordinate components (8). Parameters
* here are symmetrical, 0 is disparity-related (all to the center), remaining 9 preserve disparity and * here are symmetrical, 0 is disparity-related (all to the center), remaining 9 preserve disparity and
* are only responsible for the "lazy eye" (last 4 are for roll): * are only responsible for the "lazy eye" (last 4 are were roll, now differential zooms are added):
* *
* |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘| * |↘ ↙| |↘ ↗| |↗ ↘| |↙ ↘| |↙ ↗| |↖ ↘|
* 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖| * 0: |↗ ↖| 1: |↙ ↖| 2: |↖ ↙| 3: |↖ ↗| 4: |↗ ↙| 5: |↘ ↖|
* *
* @param port_coord_deriv result of getPortsCoordinatesAndDerivatives: first index: port0x, port0y... port3y, * @param port_coord_deriv result of getPortsCoordinatesAndDerivatives: first index: port0x, port0y... port3y,
* second index - parameters [tilt0, tilt1, ..., roll3] * second index - parameters [tilt0, tilt1, ..., roll3]
* @param par_mask array of 10 elements - which parameters to use (normally all true or all but first * @param par_mask array of 10 elements - which parameters to use (normally all true or all but first
* @return * @return
*/ */
public double [][] getJtPartial( public double [][] getJtPartial(
double [][] port_coord_deriv, double [][] port_coord_deriv,
boolean [] par_mask) boolean [] par_mask)
{ {
int num_pars = 0; int num_pars = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) { for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
num_pars++; num_pars++;
} }
...@@ -600,7 +580,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -600,7 +580,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
boolean [] par_mask) boolean [] par_mask)
{ {
double [][] tar_to_sym = dSym_j_dTar_i(); double [][] tar_to_sym = dSym_j_dTar_i();
int num_pars = 0; int num_pars = 0;
for (int npar = 0; npar < vector.length; npar++) if ((par_mask==null) || par_mask[npar]) { for (int npar = 0; npar < vector.length; npar++) if ((par_mask==null) || par_mask[npar]) {
num_pars++; num_pars++;
} }
...@@ -608,7 +588,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -608,7 +588,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
int opar = 0; int opar = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) { for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int i = 0; i < tar_array.length; i++){ for (int i = 0; i < tar_array.length; i++){
sym_array[opar] += tar_array[i] * tar_to_sym[i][npar]; sym_array[opar] += tar_array[i] * tar_to_sym[i][npar];
} }
opar++; opar++;
} }
...@@ -624,17 +604,17 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -624,17 +604,17 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
for (int npar = 0; npar < LENGTH; npar++) { for (int npar = 0; npar < LENGTH; npar++) {
int spar = 0; int spar = 0;
for (int i = 0; i < LENGTH; i++) if ((par_mask==null) || par_mask[i]){ for (int i = 0; i < LENGTH; i++) if ((par_mask==null) || par_mask[i]){
tar_array[npar] += sym_array[spar] * sym_to_tar[i][npar]; tar_array[npar] += sym_array[spar] * sym_to_tar[i][npar];
spar++; spar++;
} }
} }
return tar_array; return tar_array;
} }
} }
public void setDistortion( public void setDistortion(
double focalLength, double focalLength,
double distortionC, double distortionC,
...@@ -734,7 +714,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -734,7 +714,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
common_height /= numSensors; common_height /= numSensors;
// double [][] // double [][]
this.XYZ_he = new double [numSensors][3]; // after heading, then elevation rotation this.XYZ_he = new double [numSensors][3]; // after heading, then elevation rotation
/* /*
rotate by phi around C2Y:Vc3= R3*Vc2 rotate by phi around C2Y:Vc3= R3*Vc2
| cos(phi) 0 -sin(phi) | |X| | cos(phi) 0 -sin(phi) | |X|
| 0 1 0 | * |Y| | 0 1 0 | * |Y|
...@@ -744,7 +724,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -744,7 +724,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double s_head= Math.sin(heading*Math.PI/180); double s_head= Math.sin(heading*Math.PI/180);
double [][] aR_head={{c_head,0.0,-s_head},{0.0,1.0,0.0},{s_head,0.0,c_head}}; double [][] aR_head={{c_head,0.0,-s_head},{0.0,1.0,0.0},{s_head,0.0,c_head}};
Matrix R_head=new Matrix(aR_head); Matrix R_head=new Matrix(aR_head);
/* /*
rotate by theta around C1X:Vc2= R2*Vc1 rotate by theta around C1X:Vc2= R2*Vc1
| 1 0 0 | |X| | 1 0 0 | |X|
| 0 cos(theta) -sin(theta) | * |Y| | 0 cos(theta) -sin(theta) | * |Y|
...@@ -754,7 +734,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -754,7 +734,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double s_elev= Math.sin(elevation*Math.PI/180); double s_elev= Math.sin(elevation*Math.PI/180);
double [][] aR_elev={{1.0,0.0,0.0},{0.0,c_elev, -s_elev},{0.0, s_elev, c_elev}}; double [][] aR_elev={{1.0,0.0,0.0},{0.0,c_elev, -s_elev},{0.0, s_elev, c_elev}};
Matrix R_elev=new Matrix(aR_elev); Matrix R_elev=new Matrix(aR_elev);
Matrix R_head_elev = R_elev.times(R_head); Matrix R_head_elev = R_elev.times(R_head);
for (int i = 0; i<numSensors; i++){ for (int i = 0; i<numSensors; i++){
double [][] aXYZi_ccs = { double [][] aXYZi_ccs = {
...@@ -763,13 +743,13 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -763,13 +743,13 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{ forward[i] - common_forward}}; { forward[i] - common_forward}};
Matrix XYZi_ccs = new Matrix(aXYZi_ccs); Matrix XYZi_ccs = new Matrix(aXYZi_ccs);
Matrix mXYZ_he = R_head_elev.times(XYZi_ccs); Matrix mXYZ_he = R_head_elev.times(XYZi_ccs);
for (int j = 0; j<3;j++) this.XYZ_he[i][j] = mXYZ_he.get(j, 0); for (int j = 0; j<3;j++) this.XYZ_he[i][j] = mXYZ_he.get(j, 0);
} }
// Calculate average radius // Calculate average radius
cameraRadius = 0; cameraRadius = 0;
for (int i = 0; i < numSensors; i++){ for (int i = 0; i < numSensors; i++){
cameraRadius += this.XYZ_he[i][0] * this.XYZ_he[i][0] + this.XYZ_he[i][1] * this.XYZ_he[i][1]; cameraRadius += this.XYZ_he[i][0] * this.XYZ_he[i][0] + this.XYZ_he[i][1] * this.XYZ_he[i][1];
} }
cameraRadius = Math.sqrt(cameraRadius/numSensors); cameraRadius = Math.sqrt(cameraRadius/numSensors);
} }
...@@ -778,8 +758,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -778,8 +758,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
if (numSensors != 4 ){ if (numSensors != 4 ){
throw new IllegalArgumentException ("adjustSquare() is valid only for quad-cameras, numSensors="+numSensors); throw new IllegalArgumentException ("adjustSquare() is valid only for quad-cameras, numSensors="+numSensors);
} }
this.disparityRadius = Math.sqrt(2.0) * this.cameraRadius; this.disparityRadius = Math.sqrt(2.0) * this.cameraRadius;
double Sx = - XYZ_he[0][1] + XYZ_he[1][0] - XYZ_he[2][0] + XYZ_he[3][1]; double Sx = - XYZ_he[0][1] + XYZ_he[1][0] - XYZ_he[2][0] + XYZ_he[3][1];
double Sy = - XYZ_he[0][0] - XYZ_he[1][1] + XYZ_he[2][1] + XYZ_he[3][0]; double Sy = - XYZ_he[0][0] - XYZ_he[1][1] + XYZ_he[2][1] + XYZ_he[3][0];
double psi = 0.25*Math.PI - Math.atan2(Sy, Sx); double psi = 0.25*Math.PI - Math.atan2(Sy, Sx);
common_roll = psi*180/Math.PI; common_roll = psi*180/Math.PI;
...@@ -807,11 +787,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -807,11 +787,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{this.XYZ_he[i][2]}}; {this.XYZ_he[i][2]}};
Matrix mXYZi_he = new Matrix(aXYZi_he); Matrix mXYZi_he = new Matrix(aXYZi_he);
Matrix mXYZ_her = R_roll.times(mXYZi_he); Matrix mXYZ_her = R_roll.times(mXYZi_he);
for (int j = 0; j<3;j++) this.XYZ_her[i][j] = mXYZ_her.get(j, 0); for (int j = 0; j<3;j++) this.XYZ_her[i][j] = mXYZ_her.get(j, 0);
for (int j = 0; j<2;j++) this.rXY[i][j] = this.XYZ_her[i][j]/this.disparityRadius; for (int j = 0; j<2;j++) this.rXY[i][j] = this.XYZ_her[i][j]/this.disparityRadius;
} }
} }
public void listGeometryCorrection(boolean showAll){ public void listGeometryCorrection(boolean showAll){
System.out.println("'=== Constant parameters ==="); System.out.println("'=== Constant parameters ===");
System.out.println("pixelCorrectionWidth =\t"+ pixelCorrectionWidth+"\tpix"); System.out.println("pixelCorrectionWidth =\t"+ pixelCorrectionWidth+"\tpix");
...@@ -837,7 +817,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -837,7 +817,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
System.out.print ("roll = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+roll[i]); System.out.println("\tdegrees"); System.out.print ("roll = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+roll[i]); System.out.println("\tdegrees");
System.out.print ("px0 = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+pXY0[i][0]); System.out.println("\tpix"); System.out.print ("px0 = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+pXY0[i][0]); System.out.println("\tpix");
System.out.print ("py0 = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+pXY0[i][1]); System.out.println("\tpix"); System.out.print ("py0 = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+pXY0[i][1]); System.out.println("\tpix");
System.out.println("'=== Common calculated parameters ==="); System.out.println("'=== Common calculated parameters ===");
System.out.println("common_right =\t"+common_right + "\tmm"); System.out.println("common_right =\t"+common_right + "\tmm");
System.out.println("common_forward =\t"+common_forward + "\tmm"); System.out.println("common_forward =\t"+common_forward + "\tmm");
...@@ -856,7 +836,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -856,7 +836,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
System.out.print ("Y_her ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+XYZ_her[i][1]); System.out.println("\tmm"); System.out.print ("Y_her ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+XYZ_her[i][1]); System.out.println("\tmm");
System.out.print ("Z_her ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+XYZ_her[i][2]); System.out.println("\tmm"); System.out.print ("Z_her ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+XYZ_her[i][2]); System.out.println("\tmm");
} }
System.out.println("'=== Individual calculated parameters ==="); System.out.println("'=== Individual calculated parameters ===");
System.out.print ("residual_roll = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+(roll[i]-common_roll));System.out.println("\tdegrees"); System.out.print ("residual_roll = "); for (int i = 0; i < numSensors;i++) System.out.print("\t"+(roll[i]-common_roll));System.out.println("\tdegrees");
System.out.print ("X_rel ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+rXY[i][0]); System.out.println("\trelative to disparityRadius"); System.out.print ("X_rel ="); for (int i = 0; i < numSensors;i++) System.out.print("\t"+rXY[i][0]); System.out.println("\trelative to disparityRadius");
...@@ -871,12 +851,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -871,12 +851,12 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
public double getDisparityFromZ(double z){ public double getDisparityFromZ(double z){
return (1000.0 * SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / this.pixelSize) / z; return (1000.0 * SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / this.pixelSize) / z;
} }
public double getFOVPix(){ // get ratio of 1 pixel X/Y to Z (distance to object) public double getFOVPix(){ // get ratio of 1 pixel X/Y to Z (distance to object)
return 0.001 * this.pixelSize / this.focalLength; return 0.001 * this.pixelSize / this.focalLength;
} }
public double getFOVWidth(){ // get FOV ratio: width to distance public double getFOVWidth(){ // get FOV ratio: width to distance
return this.pixelCorrectionWidth * 0.001 * this.pixelSize / this.focalLength; return this.pixelCorrectionWidth * 0.001 * this.pixelSize / this.focalLength;
} }
...@@ -888,8 +868,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -888,8 +868,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{ {
return ( 0.001 * this.pixelSize) / this.focalLength; return ( 0.001 * this.pixelSize) / this.focalLength;
} }
/** /**
* Get real world coordinates from pixel coordinates and nominal disparity * Get real world coordinates from pixel coordinates and nominal disparity
* @param px horizontal pixel coordinate (right) * @param px horizontal pixel coordinate (right)
...@@ -916,11 +896,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -916,11 +896,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double [] xyz = {x,y,z}; double [] xyz = {x,y,z};
return xyz; return xyz;
} }
/** /**
* Find disparity for the intersection of the view ray (px, py) and a real-world plane orthogonal through the end of the * Find disparity for the intersection of the view ray (px, py) and a real-world plane orthogonal through the end of the
* vector norm_xyz * vector norm_xyz
* @param norm_xyz vector from the origin (camera) orthogonal to the plane, length is a distance to the plane * @param norm_xyz vector from the origin (camera) orthogonal to the plane, length is a distance to the plane
* @param px pixel coordinate horizontal * @param px pixel coordinate horizontal
* @param py pixel coordinate vertical * @param py pixel coordinate vertical
* @param correctDistortions true for lens distortion correction, false otherwise * @param correctDistortions true for lens distortion correction, false otherwise
...@@ -944,9 +924,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -944,9 +924,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (0.001*this.pixelSize); // "+" - near, "-" far double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (0.001*this.pixelSize); // "+" - near, "-" far
double vect_dot_norm = (x * norm_xyz[0]) + (y * norm_xyz[1]) + (z * norm_xyz[2]); double vect_dot_norm = (x * norm_xyz[0]) + (y * norm_xyz[1]) + (z * norm_xyz[2]);
double norm_dot_norm = (norm_xyz[0] * norm_xyz[0]) + (norm_xyz[1] * norm_xyz[1]) + (norm_xyz[2] * norm_xyz[2]); double norm_dot_norm = (norm_xyz[0] * norm_xyz[0]) + (norm_xyz[1] * norm_xyz[1]) + (norm_xyz[2] * norm_xyz[2]);
return vect_dot_norm / norm_dot_norm; return vect_dot_norm / norm_dot_norm;
} }
/* Just for testing using delta instead of d */ /* Just for testing using delta instead of d */
public double [][] getWorldJacobian( public double [][] getWorldJacobian(
double px, double px,
...@@ -963,15 +943,15 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -963,15 +943,15 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
dpxpy[i] += delta; dpxpy[i] += delta;
double [] xyz = getWorldCoordinates(dpxpy[1],dpxpy[2],dpxpy[0],correctDistortions); double [] xyz = getWorldCoordinates(dpxpy[1],dpxpy[2],dpxpy[0],correctDistortions);
for (int j = 0; j<3; j++){ for (int j = 0; j<3; j++){
jacobian[j][i] = (xyz[j] - xyz0[j])/delta; jacobian[j][i] = (xyz[j] - xyz0[j])/delta;
} }
} }
return jacobian; return jacobian;
} }
/** /**
* Get jacobian matrix - derivatives of real world [x,y,z] (in meters, right, up, to camera) by [disparity, px,py] (in pixels disparity, right, down) * Get jacobian matrix - derivatives of real world [x,y,z] (in meters, right, up, to camera) by [disparity, px,py] (in pixels disparity, right, down)
* @param px horizontal pixel coordinate (right) * @param px horizontal pixel coordinate (right)
* @param py vertical pixel coordinate (down) * @param py vertical pixel coordinate (down)
* @param disparity nominal disparity (pixels) * @param disparity nominal disparity (pixels)
...@@ -987,59 +967,59 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -987,59 +967,59 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{ {
double pXcd = px - 0.5 * this.pixelCorrectionWidth; double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight; double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double d_pXcd_d_px = 1.0; double d_pXcd_d_px = 1.0;
double d_pYcd_d_py = 1.0; double d_pYcd_d_py = 1.0;
double pXcd2_pYcd2 = pXcd*pXcd + pYcd*pYcd; double pXcd2_pYcd2 = pXcd*pXcd + pYcd*pYcd;
double rD = Math.sqrt(pXcd2_pYcd2)*0.001*this.pixelSize; // distorted radius in a virtual center camera double rD = Math.sqrt(pXcd2_pYcd2)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rrD = rD / this.distortionRadius; double rrD = rD / this.distortionRadius;
double d_rRD_d_px = pXcd * rD / pXcd2_pYcd2 / this.distortionRadius; double d_rRD_d_px = pXcd * rD / pXcd2_pYcd2 / this.distortionRadius;
double d_rRD_d_py = pYcd * rD / pXcd2_pYcd2 / this.distortionRadius; double d_rRD_d_py = pYcd * rD / pXcd2_pYcd2 / this.distortionRadius;
double rND2R = correctDistortions?(getRByRDist(rrD, false)): 1.0; double rND2R = correctDistortions?(getRByRDist(rrD, false)): 1.0;
double rrND = rND2R * rrD; // relative to distortion radius non-distorted radius double rrND = rND2R * rrD; // relative to distortion radius non-distorted radius
// k = rD/r // k = rD/r
double d_k_d_rrND = correctDistortions?getDerivRDistFromR(rrND):0.0; double d_k_d_rrND = correctDistortions?getDerivRDistFromR(rrND):0.0;
double d_rND2R_d_rrD = - rND2R * rND2R * d_k_d_rrND / ( d_k_d_rrND * rrND + 1.0/ rND2R); // rrND); double d_rND2R_d_rrD = - rND2R * rND2R * d_k_d_rrND / ( d_k_d_rrND * rrND + 1.0/ rND2R); // rrND);
double d_rND2R_d_px = d_rND2R_d_rrD * d_rRD_d_px; double d_rND2R_d_px = d_rND2R_d_rrD * d_rRD_d_px;
double d_rND2R_d_py = d_rND2R_d_rrD * d_rRD_d_py; double d_rND2R_d_py = d_rND2R_d_rrD * d_rRD_d_py;
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight) double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels double pYc = pYcd * rND2R; // in pixels
double d_pXc_d_px = pXcd * d_rND2R_d_px + d_pXcd_d_px * rND2R; // incorrect double d_pXc_d_px = pXcd * d_rND2R_d_px + d_pXcd_d_px * rND2R; // incorrect
double d_pYc_d_py = pYcd * d_rND2R_d_py + d_pYcd_d_py * rND2R; // incorrect double d_pYc_d_py = pYcd * d_rND2R_d_py + d_pYcd_d_py * rND2R; // incorrect
double d_pXc_d_py = pXcd * d_rND2R_d_py; // incorrect (too small)? double d_pXc_d_py = pXcd * d_rND2R_d_py; // incorrect (too small)?
double d_pYc_d_px = pYcd * d_rND2R_d_px; // incorrect (too small)? double d_pYc_d_px = pYcd * d_rND2R_d_px; // incorrect (too small)?
double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (disparity * 0.001*this.pixelSize); // "+" - near, "-" far double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (disparity * 0.001*this.pixelSize); // "+" - near, "-" far
double kx = SCENE_UNITS_SCALE * this.disparityRadius / disparity; double kx = SCENE_UNITS_SCALE * this.disparityRadius / disparity;
double ky = -SCENE_UNITS_SCALE * this.disparityRadius / disparity; double ky = -SCENE_UNITS_SCALE * this.disparityRadius / disparity;
double x = kx * pXc; double x = kx * pXc;
double y = ky * pYc; double y = ky * pYc;
double d_z_d_disparity = -z / disparity; double d_z_d_disparity = -z / disparity;
double d_x_d_disparity = -x / disparity; double d_x_d_disparity = -x / disparity;
double d_y_d_disparity = -y / disparity; double d_y_d_disparity = -y / disparity;
// d_z_d_px == d_z_d_py ==0 // d_z_d_px == d_z_d_py ==0
double [][] jacobian = double [][] jacobian =
{ {d_x_d_disparity, kx * d_pXc_d_px, kx * d_pXc_d_py}, { {d_x_d_disparity, kx * d_pXc_d_px, kx * d_pXc_d_py},
{d_y_d_disparity, ky * d_pYc_d_px, ky * d_pYc_d_py}, {d_y_d_disparity, ky * d_pYc_d_px, ky * d_pYc_d_py},
{d_z_d_disparity, 0.0, 0.0}}; {d_z_d_disparity, 0.0, 0.0}};
return jacobian; // xyz; return jacobian; // xyz;
} }
/** /**
* Get pixel disparity and coordinates from the real world coordinates (in meters) * Get pixel disparity and coordinates from the real world coordinates (in meters)
* @param xyz real world coordinates {x, y, z} in meters (right up, towards camera) * @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
...@@ -1054,8 +1034,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1054,8 +1034,8 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double y = xyz[1]; double y = xyz[1];
double z = xyz[2]; double z = xyz[2];
double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize); double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm
double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels
double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); // pixels
double rND = Math.sqrt(pXc*pXc + pYc*pYc)*0.001*this.pixelSize; // mm double rND = Math.sqrt(pXc*pXc + pYc*pYc)*0.001*this.pixelSize; // mm
double rD2RND = correctDistortions?getRDistByR(rND/this.distortionRadius):1.0; double rD2RND = correctDistortions?getRDistByR(rND/this.distortionRadius):1.0;
...@@ -1064,7 +1044,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1064,7 +1044,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double [] dxy = {disparity, px, py}; double [] dxy = {disparity, px, py};
return dxy; return dxy;
} }
/* Just for testing using delta instead of d */ /* Just for testing using delta instead of d */
public double [][] getImageJacobian( public double [][] getImageJacobian(
double [] xyz0, double [] xyz0,
...@@ -1078,15 +1058,15 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1078,15 +1058,15 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
xyz[i] += delta; xyz[i] += delta;
double [] dpxpy = getImageCoordinates(xyz,correctDistortions); double [] dpxpy = getImageCoordinates(xyz,correctDistortions);
for (int j = 0; j<3; j++){ for (int j = 0; j<3; j++){
jacobian[j][i] = (dpxpy[j] - dpxpy0[j])/delta; jacobian[j][i] = (dpxpy[j] - dpxpy0[j])/delta;
} }
} }
return jacobian; return jacobian;
} }
/** /**
* Get jacobian matrix - derivatives of [disparity, px,py] (in pixels disparity, right, down) by real world [x,y,z] (in meters, right, up, to camera) * Get jacobian matrix - derivatives of [disparity, px,py] (in pixels disparity, right, down) by real world [x,y,z] (in meters, right, up, to camera)
* @param xyz real world coordinates {x, y, z} in meters (right up, towards camera) * @param xyz real world coordinates {x, y, z} in meters (right up, towards camera)
* @param correctDistortions true: correct lens distortions, false - no lens distortions * @param correctDistortions true: correct lens distortions, false - no lens distortions
* @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}} * @return {{dx/ddisparity, dx/dpx, dx/dpy},{dy/ddisparity, dy/dpx, dy/dpy},{dz/ddisparity, dz/dpx, dz/dpy}}
...@@ -1104,9 +1084,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1104,9 +1084,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double z = xyz[2]; double z = xyz[2];
double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize); double disparity = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (z * 0.001*this.pixelSize);
double d_disparity_d_z = -disparity / z; // no dependence on x,y double d_disparity_d_z = -disparity / z; // no dependence on x,y
// non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)in mm
double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); double pXc = x * disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius); double pYc =-y * disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double d_pXc_d_z = -pXc / z; // pXc/disparity * d_disparity_d_z = pXc/disparity * (-disparity / z); double d_pXc_d_z = -pXc / z; // pXc/disparity * d_disparity_d_z = pXc/disparity * (-disparity / z);
...@@ -1114,7 +1094,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1114,7 +1094,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double d_pXc_d_x = disparity / (SCENE_UNITS_SCALE * this.disparityRadius); double d_pXc_d_x = disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double d_pYc_d_y = -disparity / (SCENE_UNITS_SCALE * this.disparityRadius); double d_pYc_d_y = -disparity / (SCENE_UNITS_SCALE * this.disparityRadius);
double rND2 = pXc*pXc + pYc*pYc; double rND2 = pXc*pXc + pYc*pYc;
double rrND = Math.sqrt(rND2)*0.001*this.pixelSize/this.distortionRadius; // relative to distortion radius; double rrND = Math.sqrt(rND2)*0.001*this.pixelSize/this.distortionRadius; // relative to distortion radius;
...@@ -1122,20 +1102,20 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1122,20 +1102,20 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double d_rrND_d_pXc = pXc*rrND/rND2; double d_rrND_d_pXc = pXc*rrND/rND2;
double d_rrND_d_pYc = pYc*rrND/rND2; double d_rrND_d_pYc = pYc*rrND/rND2;
double rD2RND = correctDistortions?getRDistByR(rrND):1.0; double rD2RND = correctDistortions?getRDistByR(rrND):1.0;
double d_rD2RND_d_rrND = correctDistortions?getDerivRDistFromR(rrND) : 0.0; double d_rD2RND_d_rrND = correctDistortions?getDerivRDistFromR(rrND) : 0.0;
/* /*
double d_rD2RND_d_rrND0 = correctDistortions?getDerivRDistFromR(rrND,0.000001) : 0.0; double d_rD2RND_d_rrND0 = correctDistortions?getDerivRDistFromR(rrND,0.000001) : 0.0;
if (debugLevel > 0){ if (debugLevel > 0){
System.out.println("getImageJacobian():, d_rD2RND_d_rrND0 = "+d_rD2RND_d_rrND0+" ("+0.000001+"), d_rD2RND_d_rrND="+d_rD2RND_d_rrND); System.out.println("getImageJacobian():, d_rD2RND_d_rrND0 = "+d_rD2RND_d_rrND0+" ("+0.000001+"), d_rD2RND_d_rrND="+d_rD2RND_d_rrND);
} }
*/ */
double p_px_d_x = d_pXc_d_x * (pXc * (d_rD2RND_d_rrND * d_rrND_d_pXc) + rD2RND); // / (0.001 * this.pixelSize); double p_px_d_x = d_pXc_d_x * (pXc * (d_rD2RND_d_rrND * d_rrND_d_pXc) + rD2RND); // / (0.001 * this.pixelSize);
double p_py_d_y = d_pYc_d_y * (pYc * (d_rD2RND_d_rrND * d_rrND_d_pYc) + rD2RND); // / (0.001 * this.pixelSize); double p_py_d_y = d_pYc_d_y * (pYc * (d_rD2RND_d_rrND * d_rrND_d_pYc) + rD2RND); // / (0.001 * this.pixelSize);
double p_px_d_y = pXc * (d_rD2RND_d_rrND * d_rrND_d_pYc * d_pYc_d_y); // / (0.001 * this.pixelSize); double p_px_d_y = pXc * (d_rD2RND_d_rrND * d_rrND_d_pYc * d_pYc_d_y); // / (0.001 * this.pixelSize);
double p_py_d_x = pYc * (d_rD2RND_d_rrND * d_rrND_d_pXc * d_pXc_d_x); // / (0.001 * this.pixelSize); double p_py_d_x = pYc * (d_rD2RND_d_rrND * d_rrND_d_pXc * d_pXc_d_x); // / (0.001 * this.pixelSize);
...@@ -1149,39 +1129,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1149,39 +1129,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{p_px_d_x,p_px_d_y, p_px_d_z}, {p_px_d_x,p_px_d_y, p_px_d_z},
{p_py_d_x,p_py_d_y, p_py_d_z}}; {p_py_d_x,p_py_d_y, p_py_d_z}};
return jacobian; return jacobian;
}
/**
* Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image
* and generic disparity, measured in pixels
* @param px pixel X coordinate
* @param py pixel Y coordinate
* @param disparity disparity
* @return array of per port pairs of pixel shifts
*/
public double [][] getPortsCoordinates_old(
double px,
double py,
double disparity)
{
double [][] port_coords = getPortsCoordinatesAndDerivatives_old(
getCorrVector(), // CorrVector corr_vector,
null, // boolean calc_deriv,
px, // double px,
py, // double py,
disparity); // double disparity)
// double [][] port_coords_rot = getPortsCoordinatesAndDerivatives_rot(
// getCorrVector(), // CorrVector corr_vector,
// null, // boolean calc_deriv,
// px, // double px,
// py, // double py,
// disparity); // double disparity)
return port_coords;
} }
/** /**
* Convert pixel coordinates to relative (from -1.0 to +1.0 each) to apply polynomial corrections * Convert pixel coordinates to relative (from -1.0 to +1.0 each) to apply polynomial corrections
* @param pXY pair of pixel X, pixel Y image coordinates * @param pXY pair of pixel X, pixel Y image coordinates
...@@ -1195,73 +1145,29 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1195,73 +1145,29 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
return relXY; return relXY;
} }
public double [][] getPortsCoordinates_nocorr( // old, tested
double px,
double py,
double disparity)
{
double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci = pYc - disparity * this.rXY[i][1];
// calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
double ri = rNDi* 0.001 * this.pixelSize / this.distortionRadius; // relative to distortion radius
// double rD2rND = (1.0 - distortionA8 - distortionA7 - distortionA6 - distortionA5 - distortionA - distortionB - distortionC);
double rD2rND = 1.0;
double rri = 1.0;
for (int j = 0; j < a.length; j++){
rri *= ri;
// rD2rND += a[j]*(rri - a[j]); // BUG here !!!! - fix later, will need to re-adjust all fine corr
rD2rND += a[j]*(rri - 1.0); // Fixed
}
double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND;
// individual rotate (check sign)
// double a_roll = (this.roll[i] - this.common_roll) * Math.PI/180.0 ;
double a_roll = (this.roll[i]) * Math.PI/180.0 ;
double c_roll = Math.cos(a_roll);
double s_roll = Math.sin(a_roll);
// double c_roll = Math.cos((this.roll[i] - this.common_roll) * Math.PI/180.0);
// double s_roll = Math.sin((this.roll[i] - this.common_roll) * Math.PI/180.0);
pXY[i][0] = c_roll * pXid + s_roll* pYid + this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll* pYid + this.pXY0[i][1];
}
return pXY;
}
/** /**
* Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image * Calculate pixel coordinates for each of numSensors images, for a given (px,py) of the idealized "center" (still distorted) image
* and generic disparity, measured in pixels * and generic disparity, measured in pixels
* @param corr_vector misalignment correction * @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 px pixel X coordinate * @param px pixel X coordinate
* @param py pixel Y coordinate * @param py pixel Y coordinate
* @param disparity disparity * @param disparity disparity
* @return array of per port pairs of pixel shifts * @return array of per port pairs of pixel shifts
*/ */
public double [][] getPortsCoordinatesAndDerivatives( public double [][] getPortsCoordinatesAndDerivatives(
Matrix [] rots, Matrix [] rots,
Matrix [][] deriv_rots, Matrix [][] deriv_rots,
double [][] pXYderiv, // if not null, should be double[8][] double [][] pXYderiv, // if not null, should be double[8][]
double px, double px,
double py, double py,
double disparity) double disparity)
{ {
// String dbg_s = corr_vector.toString(); // String dbg_s = corr_vector.toString();
double [][] pXY = new double [numSensors][2]; double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * this.pixelCorrectionWidth; double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight; double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
...@@ -1269,23 +1175,21 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1269,23 +1175,21 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight) double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels double pYc = pYcd * rND2R; // in pixels
double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8}; double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
// double corr_scale = focalLength/(0.001*pixelSize);
double fl_pix = focalLength/(0.001*pixelSize); // focal length in pixels double fl_pix = focalLength/(0.001*pixelSize); // focal length in pixels
double ri_scale = 0.001 * this.pixelSize / this.distortionRadius; double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
// Matrix [] rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
for (int i = 0; i < numSensors; i++){ for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor // non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * this.rXY[i][0]; // in pixels double pXci0 = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * this.rXY[i][1]; double pYci0 = pYc - disparity * this.rXY[i][1];
// TODO: convert to true rotations // TODO: convert to true rotations
double [][] avi = {{pXci0}, {pYci0},{fl_pix}}; double [][] avi = {{pXci0}, {pYci0},{fl_pix}};
Matrix vi = new Matrix(avi); // non-distorted sensor channel view vector in pixels (z -along the common axis) Matrix vi = new Matrix(avi); // non-distorted sensor channel view vector in pixels (z -along the common axis)
Matrix rvi = rots[i].times(vi); Matrix rvi = rots[i].times(vi);
double norm_z = fl_pix/rvi.get(2, 0); double norm_z = fl_pix/rvi.get(2, 0);
double pXci = rvi.get(0, 0) * norm_z; double pXci = rvi.get(0, 0) * norm_z;
double pYci = rvi.get(1, 0) * norm_z; double pYci = rvi.get(1, 0) * norm_z;
// calculate back to distorted // calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)"); // Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
...@@ -1297,368 +1201,90 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1297,368 +1201,90 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
rri *= ri; rri *= ri;
rD2rND += a[j]*(rri - 1.0); // Fixed rD2rND += a[j]*(rri - 1.0); // Fixed
} }
double pXid = pXci * rD2rND; double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND; double pYid = pYci * rD2rND;
pXY[i][0] = pXid + this.pXY0[i][0]; pXY[i][0] = pXid + this.pXY0[i][0];
pXY[i][1] = pYid + this.pXY0[i][1]; pXY[i][1] = pYid + this.pXY0[i][1];
if (pXYderiv != null) { if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH]; pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH]; pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
Matrix drvi_daz = deriv_rots[i][0].times(vi); Matrix drvi_daz = deriv_rots[i][0].times(vi);
Matrix drvi_dtl = deriv_rots[i][1].times(vi); Matrix drvi_dtl = deriv_rots[i][1].times(vi);
Matrix drvi_drl = deriv_rots[i][2].times(vi); Matrix drvi_drl = deriv_rots[i][2].times(vi);
Matrix drvi_dzm = deriv_rots[i][3].times(vi);
double dpXci_dazimuth = drvi_daz.get(0, 0) * norm_z - pXci * drvi_daz.get(2, 0) / rvi.get(2, 0); double dpXci_dazimuth = drvi_daz.get(0, 0) * norm_z - pXci * drvi_daz.get(2, 0) / rvi.get(2, 0);
double dpYci_dazimuth = drvi_daz.get(1, 0) * norm_z - pYci * drvi_daz.get(2, 0) / rvi.get(2, 0); double dpYci_dazimuth = drvi_daz.get(1, 0) * norm_z - pYci * drvi_daz.get(2, 0) / rvi.get(2, 0);
double dpXci_dtilt = drvi_dtl.get(0, 0) * norm_z - pXci * drvi_dtl.get(2, 0) / rvi.get(2, 0); double dpXci_dtilt = drvi_dtl.get(0, 0) * norm_z - pXci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
double dpYci_dtilt = drvi_dtl.get(1, 0) * norm_z - pYci * drvi_dtl.get(2, 0) / rvi.get(2, 0); double dpYci_dtilt = drvi_dtl.get(1, 0) * norm_z - pYci * drvi_dtl.get(2, 0) / rvi.get(2, 0);
double dpXci_droll = drvi_drl.get(0, 0) * norm_z - pXci * drvi_drl.get(2, 0) / rvi.get(2, 0); double dpXci_droll = drvi_drl.get(0, 0) * norm_z - pXci * drvi_drl.get(2, 0) / rvi.get(2, 0);
double dpYci_droll = drvi_drl.get(1, 0) * norm_z - pYci * drvi_drl.get(2, 0) / rvi.get(2, 0); double dpYci_droll = drvi_drl.get(1, 0) * norm_z - pYci * drvi_drl.get(2, 0) / rvi.get(2, 0);
/// double dpXci_dazimuth = -corr_scale; double dpXci_dzoom = drvi_dzm.get(0, 0) * norm_z - pXci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
/// double dpYci_dtilt = corr_scale; double dpYci_dzoom = drvi_drl.get(1, 0) * norm_z - pYci * drvi_dzm.get(2, 0) / rvi.get(2, 0);
/// double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale;
/// double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale;
// double ri = ri_scale * Math.sqrt(pXci*pXci + pYci*pYci) ; // relative to distortion radius
double dri_dazimuth = ri_scale / rNDi* (pXci * dpXci_dazimuth + pYci * dpYci_dazimuth); double dri_dazimuth = ri_scale / rNDi* (pXci * dpXci_dazimuth + pYci * dpYci_dazimuth);
double dri_dtilt = ri_scale / rNDi* (pXci * dpXci_dtilt + pYci * dpYci_dtilt); double dri_dtilt = ri_scale / rNDi* (pXci * dpXci_dtilt + pYci * dpYci_dtilt);
double dri_droll = ri_scale / rNDi* (pXci * dpXci_droll + pYci * dpYci_droll); double dri_droll = ri_scale / rNDi* (pXci * dpXci_droll + pYci * dpYci_droll); // Not used anywhere ?
// TODO: verify dri_droll == 0 and remove // TODO: verify dri_droll == 0 and remove
double drD2rND_dri = 0.0; double drD2rND_dri = 0.0;
rri = 1.0; rri = 1.0;
for (int j = 0; j < a.length; j++){ for (int j = 0; j < a.length; j++){
drD2rND_dri += a[j] * (j+1) * rri; drD2rND_dri += a[j] * (j+1) * rri;
rri *= ri; rri *= ri;
} }
// double pXid = pXci * rD2rND; double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
// double pYid = pYci * rD2rND;
double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
double drD2rND_dtilt = drD2rND_dri * dri_dtilt; double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
// double drD2rND_droll = 0.0;
/// double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXid * drD2rND_dazimuth;
/// double dpYid_dazimuth = pYid * drD2rND_dazimuth;
/// double dpXid_dtilt = pXid * drD2rND_dtilt;
/// double dpYid_dtilt = dpYci_dtilt * rD2rND + pYid * drD2rND_dtilt;
//// double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXid * drD2rND_dazimuth;
//// double dpYid_dazimuth = dpYci_dazimuth * rD2rND + pYid * drD2rND_dazimuth;
//// double dpXid_dtilt = dpXci_dtilt * rD2rND + pXid * drD2rND_dtilt;
//// double dpYid_dtilt = dpYci_dtilt * rD2rND + pYid * drD2rND_dtilt;
double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXci * drD2rND_dazimuth; double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXci * drD2rND_dazimuth;
double dpYid_dazimuth = dpYci_dazimuth * rD2rND + pYci * drD2rND_dazimuth; double dpYid_dazimuth = dpYci_dazimuth * rD2rND + pYci * drD2rND_dazimuth;
double dpXid_dtilt = dpXci_dtilt * rD2rND + pXci * drD2rND_dtilt; double dpXid_dtilt = dpXci_dtilt * rD2rND + pXci * drD2rND_dtilt;
double dpYid_dtilt = dpYci_dtilt * rD2rND + pYci * drD2rND_dtilt; double dpYid_dtilt = dpYci_dtilt * rD2rND + pYci * drD2rND_dtilt;
double dpXid_droll = dpXci_droll * rD2rND; double dpXid_droll = dpXci_droll * rD2rND;
double dpYid_droll = dpYci_droll * rD2rND; double dpYid_droll = dpYci_droll * rD2rND;
/// double dxi_dazimuth = c_roll * dpXid_dazimuth + s_roll * dpYid_dazimuth; double dpXid_dzoom = dpXci_dzoom * rD2rND;
/// double dyi_dazimuth = -s_roll * dpXid_dazimuth + c_roll * dpYid_dazimuth; double dpYid_dzoom = dpYci_dzoom * rD2rND;
/// double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt;
/// double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt;
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dpYid_dazimuth;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dpYid_dazimuth;
}
}
// double dxi_droll = -s_roll * pXid + c_roll * pYid;
// double dyi_droll = -c_roll * pXid - s_roll * pYid;
pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dpXid_droll;
pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dpYid_droll;
}
}
return pXY;
}
public double [][] getPortsCoordinatesAndDerivatives_old(
CorrVector corr_vector,
double [][] pXYderiv, // if not null, should be double[8][]
double px,
double py,
double disparity)
{
// String dbg_s = corr_vector.toString();
double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
double corr_scale = focalLength/(0.001*pixelSize);
double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci = pYc - disparity * this.rXY[i][1];
// TODO: convert to true rotations
// add misalignment, for small angles express in non-distorted pixels
pXci += -corr_vector.getAzimuth(i) * corr_scale;
pYci += corr_vector.getTilt(i) * corr_scale;
// calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
double ri = rNDi* ri_scale; // relative to distortion radius
// double rD2rND = (1.0 - distortionA8 - distortionA7 - distortionA6 - distortionA5 - distortionA - distortionB - distortionC);
double rD2rND = 1.0;
double rri = 1.0;
for (int j = 0; j < a.length; j++){
rri *= ri;
rD2rND += a[j]*(rri - 1.0); // Fixed
}
double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND;
// individual rotate (check sign)
double a_roll = (this.roll[i]) * Math.PI/180.0 + corr_vector.getRoll(i) ;
double c_roll = Math.cos(a_roll);
double s_roll = Math.sin(a_roll);
pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
if (pXYderiv != null) {
pXYderiv[2 * i] = new double [CorrVector.LENGTH];
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
double dpXci_dazimuth = -corr_scale;
double dpYci_dtilt = corr_scale;
double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale;
double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale;
double drD2rND_dri = 0.0;
rri = 1.0;
for (int j = 0; j < a.length; j++){
drD2rND_dri += a[j] * (j+1) * rri;
rri *= ri;
}
double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXid * drD2rND_dazimuth;
double dpYid_dazimuth = pYid * drD2rND_dazimuth;
double dpXid_dtilt = pXid * drD2rND_dtilt;
double dpYid_dtilt = dpYci_dtilt * rD2rND + pYid * drD2rND_dtilt;
double dxi_dazimuth = c_roll * dpXid_dazimuth + s_roll * dpYid_dazimuth;
double dyi_dazimuth = -s_roll * dpXid_dazimuth + c_roll * dpYid_dazimuth;
double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt;
double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt;
// verify that d/dsym are well, symmetrical // verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){ if (i < (numSensors - 1)){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dxi_dtilt; pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dyi_dtilt; pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dxi_dazimuth; pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dyi_dazimuth; pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dpYid_dazimuth;
} else {
for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dxi_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dyi_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dxi_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dyi_dazimuth;
}
}
double dxi_droll = -s_roll * pXid + c_roll * pYid;
double dyi_droll = -c_roll * pXid - s_roll * pYid;
pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dxi_droll;
pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dyi_droll;
}
}
return pXY;
}
public double [][] getPortsCoordinatesAndDerivatives_old(
double [] dbg_a_vector, // replace actual radial distortion coefficients
CorrVector corr_vector,
double [][] pXYderiv, // if not null, should be double[8][]
double px,
double py,
double disparity)
{
double [][] pXY = new double [numSensors][2];
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1));
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
if (dbg_a_vector != null){
a = dbg_a_vector;
}
double corr_scale = focalLength/(0.001*pixelSize);
double ri_scale = 0.001 * this.pixelSize / this.distortionRadius;
for (int i = 0; i < numSensors; i++){
// non-distorted XY of the shifted location of the individual sensor
double pXci0 = pXc - disparity * this.rXY[i][0]; // in pixels
double pYci0 = pYc - disparity * this.rXY[i][1];
// add misalignment, for small angles express in non-distorted pixels
double pXci = pXci0 - corr_vector.getAzimuth(i) * corr_scale;
double pYci = pYci0 + corr_vector.getTilt(i) * corr_scale;
// calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
double ri = rNDi* ri_scale; // relative to distortion radius
// double rD2rND = (1.0 - distortionA8 - distortionA7 - distortionA6 - distortionA5 - distortionA - distortionB - distortionC);
double rD2rND = 1.0;
double rri = 1.0;
for (int j = 0; j < a.length; j++){
rri *= ri;
rD2rND += a[j]*(rri - 1.0); // Fixed
}
double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND;
// individual rotate (check sign)
// double a_roll = (this.roll[i] - this.common_roll) * Math.PI/180.0 + corr_vector.getRoll(i) ;
double a_roll = (this.roll[i]) * Math.PI/180.0 + corr_vector.getRoll(i) ;
double c_roll = Math.cos(a_roll);
double s_roll = Math.sin(a_roll);
pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
// double [][] pXYderiv = calc_deriv? new double [numSensors*2][CorrVector.LENGTH] : null;
if (pXYderiv != null) { pXYderiv[2 * i + 0][CorrVector.ZOOM_INDEX+i] = dpXid_dzoom;
pXYderiv[2 * i] = new double [CorrVector.LENGTH]; pXYderiv[2 * i + 1][CorrVector.ZOOM_INDEX+i] = dpYid_dzoom;
pXYderiv[2 * i+1] = new double [CorrVector.LENGTH];
double dpXci_dazimuth = -corr_scale;
double dpYci_dtilt = corr_scale;
double dri_dazimuth = pXci/rNDi * dpXci_dazimuth * ri_scale;
double dri_dtilt = pYci/rNDi * dpYci_dtilt * ri_scale;
double drD2rND_dri = 0.0;
rri = 1.0;
for (int j = 0; j < a.length; j++){
drD2rND_dri += a[j] * (j+1) * rri;
rri *= ri;
}
double drD2rND_dazimuth = drD2rND_dri * dri_dazimuth;
double drD2rND_dtilt = drD2rND_dri * dri_dtilt;
// TODO: understand! hack - testing seems to work, but why?
drD2rND_dazimuth /= rD2rND;
drD2rND_dtilt /= rD2rND;
// double pXid = pXci * rD2rND;
// double pYid = pYci * rD2rND;
double dpXid_dazimuth = dpXci_dazimuth * rD2rND + pXid * drD2rND_dazimuth;
double dpYid_dazimuth = pYid * drD2rND_dazimuth;
double dpXid_dtilt = pXid * drD2rND_dtilt;
double dpYid_dtilt = dpYci_dtilt * rD2rND + pYid * drD2rND_dtilt;
// pXY[i][0] = c_roll * pXid + s_roll * pYid + this.pXY0[i][0];
// pXY[i][1] = -s_roll * pXid + c_roll * pYid + this.pXY0[i][1];
double dxi_dazimuth = c_roll * dpXid_dazimuth + s_roll * dpYid_dazimuth;
double dyi_dazimuth = -s_roll * dpXid_dazimuth + c_roll * dpYid_dazimuth;
double dxi_dtilt = c_roll * dpXid_dtilt + s_roll * dpYid_dtilt;
double dyi_dtilt = -s_roll * dpXid_dtilt + c_roll * dpYid_dtilt;
// verify that d/dsym are well, symmetrical
if (i < (numSensors - 1)){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+i] = dxi_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+i] = dyi_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+i] = dxi_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+i] = dyi_dazimuth;
} else { } else {
for (int j = 0; j < (numSensors - 1); j++){ for (int j = 0; j < (numSensors - 1); j++){
pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dxi_dtilt; pXYderiv[2 * i + 0][CorrVector.TILT_INDEX+j] = -dpXid_dtilt;
pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dyi_dtilt; pXYderiv[2 * i + 1][CorrVector.TILT_INDEX+j] = -dpYid_dtilt;
pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dxi_dazimuth; pXYderiv[2 * i + 0][CorrVector.AZIMUTH_INDEX+j] = -dpXid_dazimuth;
pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dyi_dazimuth; pXYderiv[2 * i + 1][CorrVector.AZIMUTH_INDEX+j] = -dpYid_dazimuth;
pXYderiv[2 * i + 0][CorrVector.ZOOM_INDEX+j] = -dpXid_dzoom;
pXYderiv[2 * i + 1][CorrVector.ZOOM_INDEX+j] = -dpYid_dzoom;
} }
} }
double dxi_droll = -s_roll * pXid + c_roll * pYid; pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dpXid_droll;
double dyi_droll = -c_roll * pXid - s_roll * pYid; pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dpYid_droll;
pXYderiv[2 * i + 0][CorrVector.ROLL_INDEX+i] = dxi_droll;
pXYderiv[2 * i + 1][CorrVector.ROLL_INDEX+i] = dyi_droll;
} }
} }
return pXY; return pXY;
} }
// debug version, calculates derivatives as differences
public double [][] getPortsCoordinatesAndDerivatives_old(
double [] dbg_a_vector, // replace actual radial distortion coefficients
double delta, // 1e-6
CorrVector corr_vector,
double [][] pXYderiv, // if not null, should be double[8][]
double px,
double py,
double disparity)
{
double [][] pXYderiv0 = new double[8][];
double [][] rslt = getPortsCoordinatesAndDerivatives_old(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_vector, // CorrVector corr_vector,
pXYderiv0, // null, // false, // boolean calc_deriv,
px, // double px,
py, // double py,
disparity // double disparity
);
for (int i = 0; i < pXYderiv.length; i++){
pXYderiv[i] = new double [CorrVector.LENGTH];
}
for (int nPar = 0; nPar < CorrVector.LENGTH; nPar++){
CorrVector cv_delta_p = corr_vector.clone();
CorrVector cv_delta_m = corr_vector.clone();
cv_delta_p.vector[nPar] += 0.5 *delta;
cv_delta_m.vector[nPar] -= 0.5 *delta;
double [][] rslt_p = getPortsCoordinatesAndDerivatives_old(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
cv_delta_p, // CorrVector corr_vector,
null, // boolean calc_deriv,
px, // double px,
py, // double py,
disparity // double disparity
);
double [][] rslt_m = getPortsCoordinatesAndDerivatives_old(
dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
cv_delta_m, // CorrVector corr_vector,
null, // boolean calc_deriv,
px, // double px,
py, // double py,
disparity // double disparity
);
for (int sens = 0; sens < numSensors; sens++){
for (int dir = 0; dir < 2; dir++){
pXYderiv[2 * sens +dir][nPar] = (rslt_p[sens][dir] - rslt_m[sens][dir]) / delta;
}
}
}
return rslt;
}
public double [][] getPortsCoordinatesAndDerivatives( // uses rotations public double [][] getPortsCoordinatesAndDerivatives( // uses rotations - used in AlignmentCorrection class
double [] dbg_a_vector, // replace actual radial distortion coefficients (not currently used) double [] dbg_a_vector, // replace actual radial distortion coefficients (not currently used)
double delta, // 1e-6 double delta, // 1e-6
CorrVector corr_vector, CorrVector corr_vector,
...@@ -1669,34 +1295,30 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1669,34 +1295,30 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{ {
// slower, will re-calculate matrices for each tile, but for debug - that is OK // slower, will re-calculate matrices for each tile, but for debug - that is OK
Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices Matrix [] corr_rots = corr_vector.getRotMatrices(); // get array of per-sensor rotation matrices
/// Matrix [][] deriv_rots = corr_vector.getRotDeriveMatrices();
/// double [][] pXYderiv0 = new double[8][];
double [][] rslt = getPortsCoordinatesAndDerivatives( double [][] rslt = getPortsCoordinatesAndDerivatives(
// dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_rots, // Matrix [] rots, corr_rots, // Matrix [] rots,
null, // deriv_rots, // Matrix [][] deriv_rots, null, // deriv_rots, // Matrix [][] deriv_rots,
null, // pXYderiv0, // null, // false, // boolean calc_deriv, null, // pXYderiv0, // null, // false, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
disparity // double disparity disparity // double disparity
); );
for (int i = 0; i < pXYderiv.length; i++){ for (int i = 0; i < pXYderiv.length; i++){
pXYderiv[i] = new double [CorrVector.LENGTH]; pXYderiv[i] = new double [CorrVector.LENGTH];
} }
for (int nPar = 0; nPar < CorrVector.LENGTH; nPar++){ for (int nPar = 0; nPar < CorrVector.LENGTH; nPar++){
CorrVector cv_delta_p = corr_vector.clone(); CorrVector cv_delta_p = corr_vector.clone();
CorrVector cv_delta_m = corr_vector.clone(); CorrVector cv_delta_m = corr_vector.clone();
cv_delta_p.vector[nPar] += 0.5 *delta; cv_delta_p.vector[nPar] += 0.5 *delta;
cv_delta_m.vector[nPar] -= 0.5 *delta; cv_delta_m.vector[nPar] -= 0.5 *delta;
Matrix [] corr_rots_p = cv_delta_p.getRotMatrices(); // get array of per-sensor rotation matrices Matrix [] corr_rots_p = cv_delta_p.getRotMatrices(); // get array of per-sensor rotation matrices
Matrix [] corr_rots_m = cv_delta_m.getRotMatrices(); // get array of per-sensor rotation matrices Matrix [] corr_rots_m = cv_delta_m.getRotMatrices(); // get array of per-sensor rotation matrices
double [][] rslt_p = getPortsCoordinatesAndDerivatives( double [][] rslt_p = getPortsCoordinatesAndDerivatives(
// dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients // dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_rots_p, // Matrix [] rots, corr_rots_p, // Matrix [] rots,
null, // Matrix [][] deriv_rots, null, // Matrix [][] deriv_rots,
null, // boolean calc_deriv, null, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
...@@ -1705,7 +1327,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1705,7 +1327,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
double [][] rslt_m = getPortsCoordinatesAndDerivatives( double [][] rslt_m = getPortsCoordinatesAndDerivatives(
// dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients // dbg_a_vector, // double [] dbg_a_vector, // replace actual radial distortion coefficients
corr_rots_m, // Matrix [] rots, corr_rots_m, // Matrix [] rots,
null, // Matrix [][] deriv_rots, null, // Matrix [][] deriv_rots,
null, // boolean calc_deriv, null, // boolean calc_deriv,
px, // double px, px, // double px,
py, // double py, py, // double py,
...@@ -1719,11 +1341,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1719,11 +1341,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
return rslt; return rslt;
} }
// should return same as input if disparity==0 // should return same as input if disparity==0
public double [][] getPortsCoordinatesIdeal( public double [][] getPortsCoordinatesIdeal( // used in macro mode
double px, double px,
double py, double py,
double disparity) double disparity)
...@@ -1731,9 +1353,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1731,9 +1353,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
// reverse getPortsCoordinates // reverse getPortsCoordinates
double c_roll = 1.0; // Math.cos(( - this.common_roll) * Math.PI/180.0); double c_roll = 1.0; // Math.cos(( - this.common_roll) * Math.PI/180.0);
double s_roll = 0.0; // Math.sin(( - this.common_roll) * Math.PI/180.0); double s_roll = 0.0; // Math.sin(( - this.common_roll) * Math.PI/180.0);
double pXcd0 = px - 0.5 * this.pixelCorrectionWidth; double pXcd0 = px - 0.5 * this.pixelCorrectionWidth;
double pYcd0 = py - 0.5 * this.pixelCorrectionWidth; double pYcd0 = py - 0.5 * this.pixelCorrectionWidth;
double pXcd = c_roll * pXcd0 - s_roll* pYcd0; double pXcd = c_roll * pXcd0 - s_roll* pYcd0;
double pYcd = s_roll * pXcd0 + c_roll* pYcd0; double pYcd = s_roll * pXcd0 + c_roll* pYcd0;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1)); double rND2R=getRByRDist(rD/this.distortionRadius, (debugLevel > -1));
...@@ -1745,7 +1367,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1745,7 +1367,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
// non-distorted XY of the shifted location of the individual sensor // non-distorted XY of the shifted location of the individual sensor
double pXci = pXc - disparity * this.rXY_ideal[i][0]; // in pixels double pXci = pXc - disparity * this.rXY_ideal[i][0]; // in pixels
double pYci = pYc - disparity * this.rXY_ideal[i][1]; double pYci = pYc - disparity * this.rXY_ideal[i][1];
// calculate back to distorted // calculate back to distorted
double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels double rNDi = Math.sqrt(pXci*pXci + pYci*pYci); // in pixels
// Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A8-A7-A6-A5-A-B-C)"); // Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A8-A7-A6-A5-A-B-C)");
...@@ -1757,28 +1379,22 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1757,28 +1379,22 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
rri *= ri; rri *= ri;
rD2rND += a[j]*(rri - 1.0); rD2rND += a[j]*(rri - 1.0);
} }
double pXid = pXci * rD2rND; double pXid = pXci * rD2rND;
double pYid = pYci * rD2rND; double pYid = pYci * rD2rND;
pXY[i][0] = c_roll * pXid + s_roll* pYid + 0.5 * this.pixelCorrectionWidth; // this.pXY0[i][0]; pXY[i][0] = c_roll * pXid + s_roll* pYid + 0.5 * this.pixelCorrectionWidth; // this.pXY0[i][0];
pXY[i][1] = -s_roll * pXid + c_roll* pYid + 0.5 * this.pixelCorrectionWidth; // this.pXY0[i][1]; pXY[i][1] = -s_roll * pXid + c_roll* pYid + 0.5 * this.pixelCorrectionWidth; // this.pXY0[i][1];
} }
return pXY; return pXY;
} }
public double [][] getPortsCoordinatesIdeal( public double [][] getPortsCoordinatesIdeal(
int macro_scale, // 1 for pixels, 8 - for tiles when correlating tiles instead of the pixels int macro_scale, // 1 for pixels, 8 - for tiles when correlating tiles instead of the pixels
double px, double px,
double py, double py,
double disparity) double disparity)
{ {
// if (macro_scale == 1){
// return getPortsCoordinates(
// px * macro_scale,
// py * macro_scale,
// disparity);
// }
double [][] coords = getPortsCoordinatesIdeal( double [][] coords = getPortsCoordinatesIdeal(
px * macro_scale, px * macro_scale,
py * macro_scale, py * macro_scale,
...@@ -1791,11 +1407,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1791,11 +1407,11 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
return coords; return coords;
} }
// Copied from PixelMapping // Copied from PixelMapping
/** /**
* Calculate reverse distortion table - from pixel radius to non-distorted radius * Calculate reverse distortion table - from pixel radius to non-distorted radius
* Rdist/R=A5*R^4+A*R^3+B*R^2+C*R+(1-A5-A-B-C) * Rdist/R=A5*R^4+A*R^3+B*R^2+C*R+(1-A5-A-B-C)
* @return false if distortion is too high * @return false if distortion is too high
*/ */
public boolean calcReverseDistortionTable(){ public boolean calcReverseDistortionTable(){
...@@ -1842,14 +1458,14 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1842,14 +1458,14 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
if (bailOut) { if (bailOut) {
if (debugThis) System.out.println("calcReverseDistortionTable() i="+i+" Bailing out, drDistDr="+drDistDr); if (debugThis) System.out.println("calcReverseDistortionTable() i="+i+" Bailing out, drDistDr="+drDistDr);
return false; return false;
} }
rPrev=r; rPrev=r;
this.rByRDist[i]=r/rDist; this.rByRDist[i]=r/rDist;
if (debugThis) System.out.println("calcReverseDistortionTable() i="+i+" rDist="+rDist+" r="+r+" rPrev="+rPrev+" this.rByRDist[i]="+this.rByRDist[i]); if (debugThis) System.out.println("calcReverseDistortionTable() i="+i+" rDist="+rDist+" r="+r+" rPrev="+rPrev+" this.rByRDist[i]="+this.rByRDist[i]);
} }
return true; return true;
} }
/** /**
* Get relative (to distortion radius) distorted radius ratio to non-distorted from non-distorted * Get relative (to distortion radius) distorted radius ratio to non-distorted from non-distorted
* @param r non-distorted radius (1.0 is 2.8512mm) * @param r non-distorted radius (1.0 is 2.8512mm)
...@@ -1859,7 +1475,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1859,7 +1475,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{ {
boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0); boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC; double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
double k; double k;
if (use8){ if (use8){
k=(((((((this.distortionA8)*r+this.distortionA7)*r+this.distortionA6)*r+this.distortionA5)*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d; k=(((((((this.distortionA8)*r+this.distortionA7)*r+this.distortionA6)*r+this.distortionA5)*r + this.distortionA)*r+this.distortionB)*r+this.distortionC)*r+d;
...@@ -1870,29 +1486,27 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1870,29 +1486,27 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
/** /**
* Get derivative of relative (to distortion radius) d_Rdist/d_R from relative (to distortion radius) non-distorted radius * Get derivative of relative (to distortion radius) d_Rdist/d_R from relative (to distortion radius) non-distorted radius
* @param r non-distorted relative radius * @param r non-distorted relative radius
* @return derivative d_Rdist/d_R from (relative to relative) * @return derivative d_Rdist/d_R from (relative to relative)
*/ */
public double getDerivRDistFromR(double r) // relative to distortion radius public double getDerivRDistFromR(double r) // relative to distortion radius
{ {
boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0); boolean use8=(this.distortionA8!=0.0) || (this.distortionA7!=0.0) || (this.distortionA6!=0.0);
// double d=1.0-this.distortionA8-this.distortionA7-this.distortionA6-this.distortionA5-this.distortionA-this.distortionB-this.distortionC;
double drDistDr; double drDistDr;
if (use8){ if (use8){
drDistDr=(((((((7*this.distortionA8)*r + 6*this.distortionA7)*r + 5*this.distortionA6)*r + 4*this.distortionA5)*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC); // +d; drDistDr=(((((((7*this.distortionA8)*r + 6*this.distortionA7)*r + 5*this.distortionA6)*r + 4*this.distortionA5)*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC); // +d;
} else { } else {
// drDistDr=(((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC)*r;
drDistDr=((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC; drDistDr=((4*this.distortionA5*r + 3*this.distortionA)*r+2*this.distortionB)*r+1*this.distortionC;
} }
return drDistDr; return drDistDr;
} }
public double getDerivRDistFromR(double r, double delta) // relative to distortion radius public double getDerivRDistFromR(double r, double delta) // relative to distortion radius
{ {
return (getRDistByR(r+delta) -getRDistByR(r))/delta; return (getRDistByR(r+delta) -getRDistByR(r))/delta;
} }
public double getRByRDist(double rDist, boolean debug){ public double getRByRDist(double rDist, boolean debug){
...@@ -1921,51 +1535,5 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0. ...@@ -1921,51 +1535,5 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} }
return result; return result;
} }
/*
public double getDerivRByRDist(double rDist, boolean debug, double delta){
return (getRByRDist(rDist+delta, false) - getRByRDist(rDist, false))/delta;
}
public double getDerivRByRDist(double rDist, boolean debug){
// add exceptions;
if (this.rByRDist==null) {
calcReverseDistortionTable();
if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): this.rByRDist==null");
// return Double.NaN;
}
if (rDist<0) {
if (debug)System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): rDist<0");
return Double.NaN;
}
int index=(int) Math.floor(rDist/this.stepR);
if (index>=(this.rByRDist.length-1)) {
if (debug) System.out.println("getDerivRByRDist("+IJ.d2s(rDist,3)+"): index="+index+">="+(this.rByRDist.length-1));
return Double.NaN;
}
double result=this.rByRDist[index+1]-this.rByRDist[index];
if ((index > 0) || (index < (this.rByRDist.length-2))){ // interpolate
double a = rDist/this.stepR-index; // fractional part, 0..1.0
if ( a <= 0.5){
result= 2 * (0.5 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
a * (this.rByRDist[index+1] - this.rByRDist[index-1]);
} else {
result= 2 * (1.0 - a) * (this.rByRDist[index+1] - this.rByRDist[index]) +
(a - 0.5) * (this.rByRDist[index+2] - this.rByRDist[index]);
}
}
if (Double.isNaN(result)){
if (debug) System.out.println("this.rByRDist["+index+"]="+this.rByRDist[index]);
if (debug) System.out.println("this.rByRDist["+(index+1)+"]="+this.rByRDist[index+1]);
if (debug) System.out.println("rDist="+rDist);
if (debug) System.out.println("(rDist/this.stepR="+(rDist/this.stepR));
}
return result/this.stepR;
}
*/
} }
src/main/java/ImageDtt.java
View file @ 3f7a67de
...@@ -1566,6 +1566,10 @@ public class ImageDtt { ...@@ -1566,6 +1566,10 @@ public class ImageDtt {
showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays(); // just for debugging? showDoubleFloatArrays sdfa_instance = new showDoubleFloatArrays(); // just for debugging?
sdfa_instance.showArrays(lt_window, 2*transform_size, 2*transform_size, "lt_window"); sdfa_instance.showArrays(lt_window, 2*transform_size, 2*transform_size, "lt_window");
} }
if (globalDebugLevel > 0) {
System.out.println("macro_mode="+macro_mode);
}
/* final double [][] dbg_ports_coords = debug_ports_coordinates ? (new double[4*2*3][nTilesInChn]):null; /* final double [][] dbg_ports_coords = debug_ports_coordinates ? (new double[4*2*3][nTilesInChn]):null;
String [] dbg_titles = new String[4*2*3]; String [] dbg_titles = new String[4*2*3];
...@@ -4213,7 +4217,7 @@ public class ImageDtt { ...@@ -4213,7 +4217,7 @@ public class ImageDtt {
px = centerX - transform_size - (ce.data_x + ce.dxc_dx * kdx + ce.dxc_dy * kdy) ; // fractional left corner px = centerX - transform_size - (ce.data_x + ce.dxc_dx * kdx + ce.dxc_dy * kdy) ; // fractional left corner
py = centerY - transform_size - (ce.data_y + ce.dyc_dx * kdx + ce.dyc_dy * kdy) ; // fractional top corner py = centerY - transform_size - (ce.data_y + ce.dyc_dx * kdx + ce.dyc_dy * kdy) ; // fractional top corner
}else { }else {
System.out.println("Skipping kernels!!!"); // System.out.println("Skipping kernels!!!"); // Happens when using macro_mode, should not happen otherwise
} }
if (bdebug0){ if (bdebug0){
System.out.print(px+"\t"+py+"\t"); System.out.print(px+"\t"+py+"\t");
......
src/main/java/TileProcessor.java
View file @ 3f7a67de
This source diff could not be displayed because it is too large. You can view the blob instead.
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