Commit 36dcabff authored by Andrey Filippov's avatar Andrey Filippov

modifying for non-quad (sym <->vect and derivatives still remain)

parent 0895e5a9
......@@ -28,6 +28,8 @@ import com.elphel.imagej.common.GenericJTabbedDialog;
import Jama.Matrix;
public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
// get rid of these !
/*
private static final int LENGTH =19; // 10; // was public
private static final int LENGTH_ANGLES =10;
private static final int TILT_INDEX = 0;
......@@ -35,28 +37,39 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
private static final int ROLL_INDEX = 6;
private static final int ZOOM_INDEX = 10;
private static final int IMU_INDEX = 13; // d_tilt/dt (rad/s), d_az/dt, d_roll/dt for ERS correction, dx/dt, dy/dt, dz/dt
*/
private static final double ROT_AZ_SGN = -1.0; // sign of first sin for azimuth rotation
private static final double ROT_TL_SGN = 1.0; // sign of first sin for tilt rotation
private static final double ROT_RL_SGN = 1.0; // sign of first sin for roll rotation
private double [] vector;
private GeometryCorrection geometryCorrection;
public static final String CORR_TILT = "tilt";
public static final String CORR_AZIMUTH = "azimuth";
public static final String CORR_ROLL = "roll";
public static final String CORR_ZOOM = "zoom";
public static final String [] CORR_IMU = {"omega_tilt", "omega_azimuth", "omega_roll",
"velocity_x", "velocity_y", "velocity_z"};
// replacing static constants (they will be different for diffrent numSensors)
// 1 step - add methods, 2-nd update methods themselves
public int getLength() {return getLength (geometryCorrection.getNumSensors());}
public int getLengthAngles() {return getLengthAngles(geometryCorrection.getNumSensors());}
public int getTiltIndex() {return getTiltIndex (geometryCorrection.getNumSensors());}
public int getAzimuthIndex() {return getAzimuthIndex(geometryCorrection.getNumSensors());}
public int getRollIndex() {return getRollIndex (geometryCorrection.getNumSensors());}
public int getZoomIndex() {return getZoomIndex (geometryCorrection.getNumSensors());}
public int getIMUIndex() {return getIMUIndex (geometryCorrection.getNumSensors());}
public int getLength() {return getLength (getNumSensors());}
public int getLengthAngles() {return getLengthAngles(getNumSensors());}
public int getTiltIndex() {return getTiltIndex (getNumSensors());}
public int getAzimuthIndex() {return getAzimuthIndex(getNumSensors());}
public int getRollIndex() {return getRollIndex (getNumSensors());}
public int getZoomIndex() {return getZoomIndex (getNumSensors());}
public int getIMUIndex() {return getIMUIndex (getNumSensors());}
public double getRotAzSgn() {return ROT_AZ_SGN;}
public double getRotTlSgn() {return ROT_TL_SGN;}
public double getRotRlzSgn() {return ROT_RL_SGN;}
public double[] getVector() {return vector;}
// static that (will) use number of subcameras
public static int getLength (int num_chn) {return 4 * num_chn + 4;}
public static int getCamerasFromEV(int ev_length) {return (ev_length + 3 - CORR_IMU.length)/4;}
public static int getLength (int num_chn) {return 4 * num_chn - 3 + CORR_IMU.length;}
public static int getLengthAngles (int num_chn) {return 3 * num_chn - 2;}
public static int getTiltIndex (int num_chn) {return 0;}
public static int getAzimuthIndex (int num_chn) {return num_chn - 1;}
......@@ -66,31 +79,38 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
// public static double getRotAzSgn(int num_chn) {return ROT_AZ_SGN;}
// public static double getRotTlSgn(int num_chn) {return ROT_TL_SGN;}
// public static double getRotRlzSgn(int num_chn) {return ROT_RL_SGN;}
public SymmVectorsSet symmVectorsSet;
public int getNumSensors() {
return geometryCorrection.getNumSensors();
}
public static String [] getCorrNames(int num_chn) {
String [] corr_names = new String[getLength(num_chn)];
for (int n = 0; n < num_chn; n++) {
if (n < (num_chn - 1)) {
corr_names[getTiltIndex(num_chn) + n]= "tilt" + n;
corr_names[getAzimuthIndex(num_chn) + n]= "azimuth" + n;
corr_names[getZoomIndex(num_chn) + n]= "zoom" + n;
corr_names[getTiltIndex(num_chn) + n]= CORR_TILT + n;
corr_names[getAzimuthIndex(num_chn) + n]= CORR_AZIMUTH + n;
corr_names[getZoomIndex(num_chn) + n]= CORR_ZOOM + n;
}
corr_names[getRollIndex(num_chn) + n]= "roll" + n;
corr_names[getRollIndex(num_chn) + n]= CORR_ROLL + n;
}
for (int i = 0; i < CORR_IMU.length; i++) {
corr_names[getIMUIndex(num_chn) + i]= CORR_IMU[i];
}
corr_names[getIMUIndex(num_chn) + 0]= "omega_tilt";
corr_names[getIMUIndex(num_chn) + 1]= "omega_azimuth";
corr_names[getIMUIndex(num_chn) + 2]= "omega_roll";
corr_names[getIMUIndex(num_chn) + 3]= "velocity_x";
corr_names[getIMUIndex(num_chn) + 4]= "velocity_y";
corr_names[getIMUIndex(num_chn) + 5]= "velocity_z";
return corr_names;
}
public String [] getCorrNames() {
return getCorrNames(geometryCorrection.getNumSensors());
return getCorrNames(getNumSensors());
}
public Matrix [] getRotMatrices(Matrix rigMatrix)// USED in lwir
{
Matrix [] rots = getRotMatrices();
......@@ -360,12 +380,11 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
return rot_derivs;
}
public CorrVector (GeometryCorrection geometryCorrection)// USED in lwir
{
this.vector = new double[LENGTH];
int num_sensors = geometryCorrection.getNumSensors();
this.symmVectorsSet = SymmVector.getSymmVectorsSet (num_sensors);
this.vector = new double[getLength(num_sensors)];
this.geometryCorrection = geometryCorrection;
}
......@@ -374,65 +393,28 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
double [] sym_vector,
boolean [] par_mask)
{
int num_sensors = geometryCorrection.getNumSensors();
this.symmVectorsSet = SymmVector.getSymmVectorsSet (num_sensors);
this.vector = toTarArray(sym_vector, par_mask);
this.geometryCorrection = geometryCorrection;
}
public CorrVector (// not used in lwir
GeometryCorrection geometryCorrection,
double tilt0, double tilt1, double tilt2,
double azimuth0, double azimuth1, double azimuth2,
double roll0, double roll1, double roll2, double roll3,
double zoom0, double zoom1, double zoom2,
double omega_tilt, double omega_azimuth, double omega_roll)
{
double [] v = {
tilt0, tilt1, tilt2,
azimuth0, azimuth1, azimuth2,
roll0, roll1, roll2, roll3,
zoom0, zoom1, zoom2,
omega_tilt, omega_azimuth, omega_roll};
this.vector = v;
this.geometryCorrection = geometryCorrection;
}
public CorrVector (
GeometryCorrection geometryCorrection,
double [] vector)
{
int num_sensors = geometryCorrection.getNumSensors();
this.symmVectorsSet = SymmVector.getSymmVectorsSet (num_sensors);
if (vector != null) {
if (vector.length != LENGTH) {
throw new IllegalArgumentException("vector.length = "+vector.length+" != "+LENGTH);// not used in lwir
if (vector.length != getLength(num_sensors)) {
throw new IllegalArgumentException("vector.length = "+vector.length+" != "+getLength(num_sensors));// not used in lwir
}
this.vector = vector;
} else {
this.vector = new double[LENGTH];// not used in lwir
this.vector = new double[getLength(num_sensors)];// not used in lwir
}
this.geometryCorrection = geometryCorrection;
}
/**
* Set subcamera corrections from a single array
* @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 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)
* @param imu angular velocities for ERS correction
*/
public CorrVector (
GeometryCorrection geometryCorrection,
double [] tilt, double [] azimuth, double [] roll, double [] zoom, double [] imu)// not used in lwir
{
double [] vector = {
tilt[0], tilt[1], tilt[2],
azimuth[0], azimuth[1], azimuth[2],
roll[0], roll[1], roll[2], roll[3],
zoom[0], zoom[1], zoom[2],
imu[0], imu[1], imu[2]};
this.vector = vector;
this.geometryCorrection = geometryCorrection;
}
public CorrVector getCorrVector(
GeometryCorrection geometryCorrection,
......@@ -455,83 +437,83 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
{
double [] v = vector.clone();
double [] rolls = getFullRolls();
for (int i = 0; i < (ZOOM_INDEX - ROLL_INDEX); i++) {
v[ROLL_INDEX + i] = rolls[i];
for (int i = 0; i < (getZoomIndex() - getRollIndex()); i++) {
v[getRollIndex() + i] = rolls[i];
}
return v;
}
public double [] toArray() // USED in lwir
{
return vector;
}
public double [] getTilts() // USED in lwir
{
double [] tilts = {vector[0], vector[1], vector[2], - (vector[0] + vector[1] +vector[2])};
return tilts;
// double [] tilts = {vector[0], vector[1], vector[2], - (vector[0] + vector[1] +vector[2])};
double [] tilts = new double [getNumSensors()];
int indx = getTiltIndex();
for (int i = 0; i < (tilts.length - 1); i++) {
tilts[i] = vector[indx + i];
tilts[tilts.length - 1] -= tilts[i];
}
public double getTilt(int indx) // not used in lwir
{
if (indx == 3) return - (vector[0] + vector[1] +vector[2]);
else return vector[0 + indx];
return tilts;
}
public double [] getAzimuths() // USED in lwir
{
double [] azimuths = {vector[3], vector[4], vector[5], -(vector[3] + vector[4] + vector[5])};
return azimuths;
// double [] azimuths = {vector[3], vector[4], vector[5], -(vector[3] + vector[4] + vector[5])};
double [] azimuths = new double [getNumSensors()];
int indx = getAzimuthIndex();
for (int i = 0; i < (azimuths.length - 1); i++) {
azimuths[i] = vector[indx + i];
azimuths[azimuths.length - 1] -= azimuths[i];
}
public double getAzimuth(int indx) // not used in lwir
{
if (indx == 3) return - (vector[3] + vector[4] +vector[5]);
else return vector[3 + indx];
return azimuths;
}
public double [] getRolls() // not used in lwir
{
double [] rolls = {vector[6],vector[7],vector[8], vector[9]};
// double [] rolls = {vector[6],vector[7],vector[8], vector[9]};
double [] rolls = new double [getNumSensors()];
int indx = getRollIndex();
for (int i = 0; i < rolls.length; i++) {
rolls[i] = vector[indx + i];
}
return rolls;
}
public double getRoll(int indx) // not used in lwir
{
return vector[6 + indx];
}
public double [] getZooms() // USED in lwir
{
double [] zooms = {vector[10], vector[11], vector[12], - (vector[10] + vector[11] +vector[12])};
return zooms;
// double [] zooms = {vector[10], vector[11], vector[12], - (vector[10] + vector[11] +vector[12])};
double [] zooms = new double [getNumSensors()];
int indx = getZoomIndex();
for (int i = 0; i < (zooms.length - 1); i++) {
zooms[i] = vector[indx + i];
zooms[zooms.length - 1] -= zooms[i];
}
public double getZoom(int indx) // not used in lwir
{
if (indx == 3) return - (vector[10] + vector[11] +vector[12]);
else return vector[10 + indx];
return zooms;
}
public double [] getIMU() {
double [] imu = {
vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],
vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
return imu;
// double [] imu = {vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
int indx = getZoomIndex();
double [] imu = new double[6];
for (int i = 0; i < imu.length; i++) {
imu[i] = vector[indx + i];
}
public double [] getIMU(int chn) {
// considering all sensors parallel, if needed process all angles
double [] imu = {
vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],
vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
return imu;
}
public void setIMU(double [] imu) {
vector [IMU_INDEX + 0] = imu[0];
vector [IMU_INDEX + 1] = imu[1];
vector [IMU_INDEX + 2] = imu[2];
vector [IMU_INDEX + 3] = imu[3];
vector [IMU_INDEX + 4] = imu[4];
vector [IMU_INDEX + 5] = imu[5];
int indx = getZoomIndex();
for (int i = 0; i < imu.length; i++) {
vector [indx + i] = imu[i];
}
}
/*
public double setZoomsFromF(double f0, double f1, double f2, double f3) { // USED in lwir
double f_avg = (f0+f1+f2+f3)/4;
vector[10] = (f0 - f_avg)/f_avg;
......@@ -539,8 +521,23 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
vector[12] = (f2 - f_avg)/f_avg;
return f_avg;
}
*/
public double setZoomsFromF(double [] f) { // USED in lwir
int indx = getZoomIndex();
// double f_avg = (f0+f1+f2+f3)/4;
double f_avg = 0;
for (int i = 0; i < f.length;i++) {
f_avg += f[i];
}
f_avg /= f.length;
for (int i = 0; i < (f.length-1); i++) {
vector[indx+i] = (f[i] - f_avg)/f_avg;
}
return f_avg;
}
// Tilts in radians, theta in degrees
/*
public double setTiltsFromThetas(double t0, double t1, double t2, double t3) { // USED in lwir
double t_avg = (t0+t1+t2+t3)/4;
vector[0] = (t0 - t_avg)*Math.PI/180.0;
......@@ -548,8 +545,22 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
vector[2] = (t2 - t_avg)*Math.PI/180.0;
return t_avg;
}
*/
public double setTiltsFromThetas(double [] t) { // USED in lwir
int indx = getTiltIndex();
double t_avg = 0;
for (int i = 0; i < t.length; i++) {
t_avg += t[i];
}
t_avg /= t.length;
for (int i = 0; i < (t.length -1); i++) {
vector[indx+i] = (t[i] - t_avg)*Math.PI/180.0;
}
return t_avg;
}
// Azimuths in radians, headings in degrees
/*
public double setAzimuthsFromHeadings(double h0, double h1, double h2, double h3) { // USED in lwir
double h_avg = (h0+h1+h2+h3)/4;
vector[3] = (h0 - h_avg)*Math.PI/180.0;
......@@ -557,8 +568,22 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
vector[5] = (h2 - h_avg)*Math.PI/180.0;
return h_avg;
}
*/
public double setAzimuthsFromHeadings(double [] h) { // USED in lwir
int indx = getAzimuthIndex();
double h_avg = 0;
for (int i = 0; i < h.length; i++) {
h_avg += h[i];
}
h_avg /= h.length;
for (int i = 0; i < (h.length -1); i++) {
vector[indx+i] = (h[i] - h_avg)*Math.PI/180.0;
}
return h_avg;
}
// Include factory calibration rolls
/*
public double [] getFullRolls() // USED in lwir
{
double d2r= Math.PI/180.0;
......@@ -569,10 +594,18 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
vector[9] + d2r * geometryCorrection.roll[3]};
return rolls;
}
public double getFullRoll(int indx) // not used in lwir
*/
public double [] getFullRolls() // USED in lwir
{
return vector[6 + indx] + geometryCorrection.roll[indx] * Math.PI/180.0;
int indx =getRollIndex();
double d2r= Math.PI/180.0;
double [] rolls = new double [geometryCorrection.roll.length];
for (int i = 0; i < rolls.length; i++) {
rolls[i] = vector[indx + i] + d2r * geometryCorrection.roll[i];
}
return rolls;
}
/**
* Return parameter value for reports
* @param indx parameter index (use CorrVector.XXX static integers)
......@@ -581,23 +614,23 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
*/
public double getExtrinsicParameterValue(int indx, boolean inPix) { // not used in lwir
if (indx <0) return Double.NaN;
if (indx < ROLL_INDEX) return vector[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // tilt and azimuth
if (indx < LENGTH_ANGLES) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // rolls
if (indx < IMU_INDEX) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // zooms
if (indx < (IMU_INDEX+2)) return vector[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < (IMU_INDEX+3)) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // omega_roll
if (indx < LENGTH) return vector[indx]* (inPix? (1000.0): 1.0); // m/s or mm/s
if (indx < getRollIndex()) return vector[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // tilt and azimuth
if (indx < getLengthAngles()) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // rolls
if (indx < getIMUIndex()) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // zooms
if (indx < (getIMUIndex()+2)) return vector[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < (getIMUIndex()+3)) return vector[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // omega_roll
if (indx < getLength()) return vector[indx]* (inPix? (1000.0): 1.0); // m/s or mm/s
return Double.NaN;
}
public double getExtrinsicSymParameterValue(int indx, boolean inPix) { // not used in lwir
double [] sym_vect = toSymArray(null);
if (indx <0) return Double.NaN;
if (indx < ROLL_INDEX) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // tilt and azimuth
if (indx < LENGTH_ANGLES) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // rolls
if (indx < IMU_INDEX) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // zooms
if (indx < (IMU_INDEX+2)) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < (IMU_INDEX+3)) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // omega_roll
if (indx < LENGTH) return sym_vect[indx]* (inPix? (1000.0): 1.0); // m/s mm/s
if (indx < getRollIndex()) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // tilt and azimuth
if (indx < getLengthAngles()) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // rolls
if (indx < getIMUIndex()) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // zooms
if (indx < (getIMUIndex()+2)) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.focalLength /geometryCorrection.pixelSize): 1.0); // omega_tilt and omega_azimuth
if (indx < (getIMUIndex()+3)) return sym_vect[indx]* (inPix? (1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize): 1.0); // omega_roll
if (indx < getLength()) return sym_vect[indx]* (inPix? (1000.0): 1.0); // m/s mm/s
return Double.NaN;
}
......@@ -608,49 +641,140 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
double [] sym_vect = toSymArray(null);
double [] v = new double [vector.length];
double [] sv = new double [vector.length];
for (int i = 0; i < ROLL_INDEX; i++){
for (int i = 0; i < getRollIndex(); i++){
v[i] = vector[i]*1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize; // tilt and azimuth
sv[i] = sym_vect[i]*1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize; // all 3 angles
}
for (int i = ROLL_INDEX; i < LENGTH_ANGLES; i++){
for (int i = getRollIndex(); i < getLengthAngles(); i++){
v[i] = vector[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // rolls
sv[i] = sym_vect[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // combined rolls
}
for (int i = LENGTH_ANGLES; i < IMU_INDEX; i++){
for (int i = getLengthAngles(); i < getIMUIndex(); i++){
v[i] = vector[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // zooms
sv[i] = sym_vect[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // zooms
}
for (int i = IMU_INDEX; i < IMU_INDEX+2; i++){
for (int i = getIMUIndex(); i < getIMUIndex()+2; i++){
v[i] = vector[i]* 1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize; // omega_tilt and omega_azimuth
sv[i] = sym_vect[i]*1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize; // omega_tilt and omega_azimuth
}
for (int i = IMU_INDEX+2; i < IMU_INDEX+3; i++){
for (int i = getIMUIndex()+2; i < getIMUIndex()+3; i++){
v[i] = vector[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // omega_roll
sv[i] = sym_vect[i]*1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize; // omega_rolls
}
for (int i = IMU_INDEX+3; i < LENGTH; i++){
for (int i = getIMUIndex()+3; i < getLength(); i++){
v[i] = vector[i] *1000.0; // *distortionRadius/pixelSize; // movement mm/s
sv[i] = sym_vect[i]*1000.0; // *distortionRadius/pixelSize; // movement mm/s
}
// ← → ↑ ↓ ⇖ ⇗ ⇘ ⇙ ↔ ↕
s = String.format("tilt (up): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of the image center)\n" , v[0], v[1], v[2], -(v[0] + v[1] + v[2]) );
s += String.format("azimuth (right): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift of the image center)\n" , v[3], v[4], v[5], -(v[3] + v[4] + v[5]) );
s += String.format("roll (CW): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift at the image half-width from the center)\n" , v[6], v[7], v[8], v[9] );
s += String.format("diff zoom (in): %8.5fpx %8.5fpx %8.5fpx %8.5fpx (shift at the image half-width from the center)\n" , v[10], v[11], v[12], -(v[10] + v[11] + v[12]) );
double [] tilts = new double[getNumSensors()];
double [] azimuths = new double[getNumSensors()];
double [] rolls = new double[getNumSensors()];
double [] zooms = new double[getNumSensors()];
int n = getNumSensors();
for (int i = 0; i < getNumSensors();i++) {
rolls[i] = v[getRollIndex()+i];
if (i < (n-1)) {
tilts[i] = v[getTiltIndex()+i];
tilts[n-1] -= tilts[i];
azimuths[i] = v[getAzimuthIndex()+i];
azimuths[n-1] -= azimuths[i];
zooms[i] = v[getZoomIndex()+i];
zooms[n-1] -= zooms[i];
}
}
s = "";
s+= "tilt (up): "; for (int i = 0; i < n; i++) s += String.format(" %8.5fpx", tilts[i]); s+=" (shift of the image center)\n";
s+= "azimuth (right):"; for (int i = 0; i < n; i++) s += String.format(" %8.5fpx", azimuths[i]); s+=" (shift of the image center)\n";
s+= "roll (CW): "; for (int i = 0; i < n; i++) s += String.format(" %8.5fpx", rolls[i]); s+=" (shift at the image half-width from the center)\n";
s+= "diff zoom (in): "; for (int i = 0; i < n; i++) s += String.format(" %8.5fpx", zooms[i]); s+=" (shift at the image half-width from the center)\n";
s += "Symmetrical vector:\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 10: |- +| 11: |+ +| 12: |+ -|\n";
if (getNumSensors() == 4) { // Use arrows for quad camera only (but update to match new
// ← → ↑ ↓ ⇖ ⇗ ⇘ ⇙ ↔ ↺ ↻
// 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 10: |- +| 11: |+ +| 12: |+ -|\n";
s += " |⇘ ⇙| |⇗ ⇘| |⇘ ⇗| |⇗ ⇖| |⇘ ⇖| |⇗ ⇙| |↻ ↻| |↺ ↻| |↺ ↺| |↺ ↻| |- +| |- -| |- +|\n";
s += "0:|⇗ ⇖|1:|⇖ ⇙|2:|⇙ ⇖|3:|⇘ ⇙|4: |⇖ ⇘|5:|⇙ ⇗|6:|↻ ↻|7:|↺ ↻|8:|↻ ↻|9:|↻ ↺|10:|- +| 11: |+ +| 12: |+ -|\n";
s += String.format(" 0:%9.6fpx 1:%8.5fpx 2:%8.5fpx 3:%8.5fpx 4:%8.5fpx 5:%8.5fpx 6:%8.5fpx 7:%8.5fpx 8:%8.5fpx 9:%8.5fpx 10: %8.5fpx 11:%8.5fpx 12:%8.5fpx\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9], sv[10], sv[11], sv[12] );
s += String.format("omega_tilt = %9.4f pix/s, omega_azimuth = %9.4f pix/s, omega_roll = %9.4f pix/s\n", sv[13], sv[14], sv[15]);
s += String.format("velocity Vx = %9.4f mm/s, Vy = %9.4f mm/s, Vz = %9.4f mm/s\n", sv[16], sv[17], sv[18]);
/*
proto_type1.length=4 proto_type1_extra.length=0 proto_type2.length=2 proto_type2_extra.length=0 proto_type3.length=0
Vector # 0 [0 | 0 | 0 | 0] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= -1.0 n=-1(-1) type1:0
Vector # 1 [1 | 1 | 1 | 1] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=5(5) type1:1
Vector # 2 [0 | 2 | 0 | 2] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=4(4) type1:2
Vector # 3 [1 | 3 | 1 | 3] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=3(3) type1:3
Vector # 4 [0 | 3 | 2 | 1] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=2(2) type2:0
Vector # 5 [1 | 0 | 3 | 2] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=1(1) type2:1
No more orthogonal vectors, current number of independent vectors = 6(of 6 needed)
Vector # 0 [+|+|+|+] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= -1.0 n=-1(-1)
Vector # 1 [-|+|+|-] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=3(3)
Vector # 2 [-|-|+|+] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=2(2)
Vector # 3 [-|+|-|+] 1.00<1.00 [1.0 | 1.0 | 1.0 | 1.0 ] l= 1.0 n=1(1)
↻ ↺
*/
} else {
// azimuth/tilts
String [] rt4 = {"↓","↷","↑", "↶"};
int [][] rt_proto = symmVectorsSet.dir_rt;
int indx = 0;
for (int i = 0; i < rt_proto.length; i++) {
s += String.format("%2d: [" , indx);
for (int j = 0; j < n; j++) {
if ((j>0) && (j%(n/4) == 0)) {
s += "|";
}
s += rt4[rt_proto[indx][i]];
}
s+=String.format("] %9.6f px", sv[indx]);
indx++;
}
// rolls
double [][] rot_proto = symmVectorsSet.rots;
for (int i = 0; i < rot_proto.length; i++) {
s += String.format("%2d: [" , indx);
for (int j = 0; j < n; j++) {
if ((j>0) && (j%(n/4) == 0)) {
s += "|";
}
if (rot_proto[indx][i] > 0) {
s+="↻";
} else if (rot_proto[indx][i] < 0) {
s+="↺";
} else {
s+="0";
}
}
s+=String.format("] %9.6f px", sv[indx]);
indx++;
}
// zooms
double [][] zoom_proto = symmVectorsSet.zooms;
for (int i = 0; i < zoom_proto.length; i++) {
s += String.format("%2d: [" , indx);
for (int j = 0; j < n; j++) {
if ((j>0) && (j%(n/4) == 0)) {
s += "|";
}
if (zoom_proto[indx][i] > 0) {
s+="+";
} else if (zoom_proto[indx][i] < 0) {
s+="-";
} else {
s+="0";
}
}
s+=String.format("] %9.6f px", sv[indx]);
indx++;
}
}
return s;
}
public boolean editVector() {
int n = getNumSensors();
boolean use_tabs = (n>4);
System.out.println(toString());
String lines[] = toString().split("\\n");
double par_scales_tlaz = 1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize;
......@@ -660,129 +784,87 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
for (String s:lines) {
gd.addMessage(s);
}
for (int i = 0; i < AZIMUTH_INDEX; i++){
gd.addNumericField("Tilt "+i+" (up)", par_scales_tlaz * vector[i], 10, 13,"pix",
if (use_tabs) gd.addTab("Tilts","Vertical (around horizontal axis perpendicular to te camera view direction) rotations in pixels for current camera parameters");
for (int i = getTiltIndex(); i < getAzimuthIndex(); i++){
gd.addNumericField("Tilt "+(i -getTiltIndex())+" (up)", par_scales_tlaz * vector[i], 10, 13,"pix",
"Vertical (around horizontal axis perpendicular to te camera view direction) rotations in pixels for current camera parameters");
}
for (int i = AZIMUTH_INDEX; i < ROLL_INDEX; i++){
gd.addNumericField("Azimuth " + (i - AZIMUTH_INDEX) + " (right)", par_scales_tlaz * vector[i], 10, 13,"pix",
if (use_tabs)gd.addTab("Azimuths","Horizontal (around vertical axis) rotations in pixels for current camera parameters");
for (int i = getAzimuthIndex(); i < getRollIndex(); i++){
gd.addNumericField("Azimuth " + (i - getAzimuthIndex()) + " (right)", par_scales_tlaz * vector[i], 10, 13,"pix",
"Horizontal (around vertical axis) rotations in pixels for current camera parameters");
}
for (int i = ROLL_INDEX; i < ZOOM_INDEX; i++){
gd.addNumericField("Roll " + (i - ROLL_INDEX) + " (clockwise)", par_scales_roll * vector[i] , 10, 13,"pix",
"Horizontal (around vertical axis) rotations in pixels for current camera parameters, 1 pixel corresponds to FoV edges");
if (use_tabs) gd.addTab("Rolls","Roll around view axis rotations in pixels for current camera parameters, 1 pixel corresponds to FoV edges");
for (int i = getRollIndex(); i < getZoomIndex(); i++){
gd.addNumericField("Roll " + (i - getRollIndex()) + " (clockwise)", par_scales_roll * vector[i] , 10, 13,"pix",
"Roll around view axis rotations in pixels for current camera parameters, 1 pixel corresponds to FoV edges");
}
for (int i = ZOOM_INDEX; i < IMU_INDEX; i++){
gd.addNumericField("Zoom " + (i - ZOOM_INDEX) + " (zoom in)", par_scales_roll * vector[i], 10, 13,"pix",
if (use_tabs) gd.addTab("Zooms","Relative (to the average) focal length modifications, 1 pixel corresponds to FoV edges");
for (int i = getZoomIndex(); i < getIMUIndex(); i++){
gd.addNumericField("Zoom " + (i - getZoomIndex()) + " (zoom in)", par_scales_roll * vector[i], 10, 13,"pix",
"Relative (to the average) focal length modifications, 1 pixel corresponds to FoV edges");
}
gd.addMessage("--- ERS correction parameters ---");
if (use_tabs) gd.addTab("ERS","ERS correction parameters");
else gd.addMessage("--- ERS correction parameters ---");
gd.addNumericField("Angular rotation azimuth (right)", par_scales_tlaz * vector[IMU_INDEX + 0], 10, 13,"pix/s",
gd.addNumericField("Angular rotation azimuth (right)", par_scales_tlaz * vector[getIMUIndex() + 0], 10, 13,"pix/s",
"Horizonatal camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (up)", par_scales_tlaz * vector[IMU_INDEX + 1], 10, 13,"pix/s",
gd.addNumericField("Angular rotation tilt (up)", par_scales_tlaz * vector[getIMUIndex() + 1], 10, 13,"pix/s",
"Vertical camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (clockwise)", par_scales_roll * vector[IMU_INDEX + 2], 10, 13,"pix/s",
gd.addNumericField("Angular rotation tilt (clockwise)", par_scales_roll * vector[getIMUIndex() + 2], 10, 13,"pix/s",
"Roll camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Linear velocity (right)", par_scales_linear * vector[IMU_INDEX + 3], 10, 13,"mm/s",
gd.addNumericField("Linear velocity (right)", par_scales_linear * vector[getIMUIndex() + 3], 10, 13,"mm/s",
"Linear movement right");
gd.addNumericField("Linear velocity (up)", par_scales_linear * vector[IMU_INDEX + 4], 10, 13,"mm/s",
gd.addNumericField("Linear velocity (up)", par_scales_linear * vector[getIMUIndex() + 4], 10, 13,"mm/s",
"Linear movement up");
gd.addNumericField("Linear velocity (forward)", par_scales_linear * vector[IMU_INDEX + 5], 10, 13,"mm/s",
gd.addNumericField("Linear velocity (forward)", par_scales_linear * vector[getIMUIndex() + 5], 10, 13,"mm/s",
"Linear movement forward");
gd.showDialog();
if (gd.wasCanceled()) return false;
for (int i = 0; i < ROLL_INDEX; i++){
for (int i = 0; i < getRollIndex(); i++){
vector[i] = gd.getNextNumber()/par_scales_tlaz;
}
for (int i = ROLL_INDEX; i < IMU_INDEX; i++){
for (int i = getRollIndex(); i < getIMUIndex(); i++){
vector[i] = gd.getNextNumber()/par_scales_roll;
}
vector[IMU_INDEX + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 2] = gd.getNextNumber()/par_scales_roll;
vector[IMU_INDEX + 3] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 4] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 5] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[getIMUIndex() + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[getIMUIndex() + 2] = gd.getNextNumber()/par_scales_roll;
vector[getIMUIndex() + 3] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 4] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 5] = gd.getNextNumber()/par_scales_linear;
return true;
}
public String toStringDegrees() // not used in lwir
{
String s;
double [] sym_vect = toSymArray(null);
double [] v = new double [vector.length];
double [] sv = new double [vector.length];
for (int i = 0; i < LENGTH; i++){
if (i < LENGTH_ANGLES) {
v[i] = vector[i]*180/Math.PI;
sv[i] = sym_vect[i]*180/Math.PI;
} else if (i < IMU_INDEX){
v[i] = vector[i];
sv[i] = sym_vect[i];
} else if (i < IMU_INDEX+3){
v[i] = vector[i]*180/Math.PI;
sv[i] = sym_vect[i]*180/Math.PI;
} else {
v[i] = vector[i]*1000;
sv[i] = sym_vect[i]*1000;
}
}
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("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 += " |⇘ ⇙| |⇘ ⇗| |⇗ ⇘| |⇙ ⇘| |⇙ ⇗| |⇖ ⇘| 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 10: |- +| 11: |+ +| 12: |+ -|\n";
s += String.format(" 0:%9.6f° 1:%8.5f° 2:%8.5f° 3:%8.5f° 4:%8.5f° 5:%8.5f° 6:%8.5f° 7:%8.5f° 8:%8.5f° 9:%8.5f° 10: %8.5f‰ 11:%8.5f‰ 12:%8.5f‰\n" ,
sv[0], sv[1], sv[2], sv[3], sv[4], sv[5], sv[6], sv[7], sv[8], sv[9], 1000*sv[10], 1000*sv[11], 1000*sv[12] );
s += String.format("omega_tilt = %9.4°/s, omega_azimuth = %9.4°/s, omega_roll = %9.4°/s\n", sv[13], sv[14], sv[15]);
s += String.format("velocity Vx = %9.4f mm/s, Vy = %9.4f mm/s, Vz = %9.4f mm/s\n", sv[16], sv[17], sv[18]);
return s;
}
public boolean editIMU() {
double par_scales_tlaz = 1000.0*geometryCorrection.focalLength/geometryCorrection.pixelSize;
double par_scales_roll = 1000.0*geometryCorrection.distortionRadius/geometryCorrection.pixelSize;
double par_scales_linear = 1000.0;
GenericJTabbedDialog gd = new GenericJTabbedDialog("Set camera angular velocities (in pix/sec)",800,300);
gd.addNumericField("Angular rotation azimuth (positive - rotate camera the right)", par_scales_tlaz * vector[IMU_INDEX + 0], 3,6,"pix/s",
gd.addNumericField("Angular rotation azimuth (positive - rotate camera the right)", par_scales_tlaz * vector[getIMUIndex() + 0], 3,6,"pix/s",
"Horizonatal camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (positive - rotate camera up)", par_scales_tlaz * vector[IMU_INDEX + 1], 3,6,"pix/s",
gd.addNumericField("Angular rotation tilt (positive - rotate camera up)", par_scales_tlaz * vector[getIMUIndex() + 1], 3,6,"pix/s",
"Vertical camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Angular rotation tilt (positive - clockwise)", par_scales_roll * vector[IMU_INDEX + 2], 3,6,"pix/s",
gd.addNumericField("Angular rotation tilt (positive - clockwise)", par_scales_roll * vector[getIMUIndex() + 2], 3,6,"pix/s",
"Roll camera rotation (reasonable value ~1000 pix/s)");
gd.addNumericField("Linear velocity (positive - right)", par_scales_linear * vector[IMU_INDEX + 3], 3,6,"mm/s",
gd.addNumericField("Linear velocity (positive - right)", par_scales_linear * vector[getIMUIndex() + 3], 3,6,"mm/s",
"Linear movement right");
gd.addNumericField("Linear velocity (positive - up)", par_scales_linear * vector[IMU_INDEX + 4], 3,6,"mm/s",
gd.addNumericField("Linear velocity (positive - up)", par_scales_linear * vector[getIMUIndex() + 4], 3,6,"mm/s",
"Linear movement up");
gd.addNumericField("Linear velocity (positive - forward)", par_scales_linear * vector[IMU_INDEX + 5], 3,6,"mm/s",
gd.addNumericField("Linear velocity (positive - forward)", par_scales_linear * vector[getIMUIndex() + 5], 3,6,"mm/s",
"Linear movement forward");
gd.showDialog();
if (gd.wasCanceled()) return false;
vector[IMU_INDEX + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[IMU_INDEX + 2] = gd.getNextNumber()/par_scales_roll;
vector[IMU_INDEX + 3] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 4] = gd.getNextNumber()/par_scales_linear;
vector[IMU_INDEX + 5] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 0] = gd.getNextNumber()/par_scales_tlaz;
vector[getIMUIndex() + 1] = gd.getNextNumber()/par_scales_tlaz;
vector[getIMUIndex() + 2] = gd.getNextNumber()/par_scales_roll;
vector[getIMUIndex() + 3] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 4] = gd.getNextNumber()/par_scales_linear;
vector[getIMUIndex() + 5] = gd.getNextNumber()/par_scales_linear;
return true;
}
......@@ -819,9 +901,9 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
for (int i = 0; i < vector.length; i++) {
double v = vector[i];
// manually reducing weights of ERS parameters
if (i >= (IMU_INDEX+3)) {
if (i >= (getIMUIndex()+3)) {
v *= 0.001; // imu mm/s
} else if (i >= IMU_INDEX) {
} else if (i >= getIMUIndex()) {
v *= 0.01; // imu mm/s
}
s2 += v*v;
......@@ -849,8 +931,8 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
}
double pix_to_rads = (0.001*geometryCorrection.pixelSize)/geometryCorrection.focalLength;
for (int i = 0; i < (geometryCorrection.numSensors -1); i++){
vector[TILT_INDEX + i] -= pix_to_rads * (pXY_shift[i][1] - pXY_avg[1]);
vector[AZIMUTH_INDEX + i] += pix_to_rads * (pXY_shift[i][0] - pXY_avg[0]);
vector[getTiltIndex() + i] -= pix_to_rads * (pXY_shift[i][1] - pXY_avg[1]);
vector[getAzimuthIndex() + i] += pix_to_rads * (pXY_shift[i][0] - pXY_avg[0]);
}
}
......@@ -952,28 +1034,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
} ;
return sym_to_tar;
}
/*
public boolean [] getParMask( // USED in lwir
boolean use_disparity,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
boolean common_roll,
boolean corr_focalLength,
boolean ers_rot, // Enable ERS correction of the camera rotation
boolean ers_lin, // Enable ERS correction of the camera linear movement
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
return getParMask( // USED in lwir
use_disparity,
use_aztilts, // Adjust azimuths and tilts excluding disparity
use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
common_roll,
corr_focalLength,
false, // boolean corr_imu,
manual_par_sel);
}
*/
public boolean [] getParMask( // USED in lwir
boolean use_disparity,
boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
......@@ -986,6 +1047,26 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
int manual_par_sel) // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
{
// SymmVectorsSet
int num_aztilts = symmVectorsSet.rt.length;
int num_rolls = symmVectorsSet.rots.length;
int num_zooms = symmVectorsSet.zooms.length;
int num_imus = 6;
boolean [] par_mask = new boolean[num_aztilts + num_rolls + num_zooms + num_imus];
par_mask[0] = use_disparity;
for (int i = 1; i < num_aztilts; i++) par_mask[i] = use_aztilts;
par_mask[num_aztilts] = common_roll;
for (int i = 1; i < num_rolls; i++) par_mask[i + num_aztilts] = use_diff_rolls;
for (int i = 1; i < num_zooms; i++) par_mask[i + num_aztilts + num_rolls] = corr_focalLength;
int imu_offset = num_aztilts + num_rolls + num_zooms;
par_mask[imu_offset + 0] = ers_rot;
par_mask[imu_offset + 1] = ers_rot;
par_mask[imu_offset + 2] = ers_rot;
par_mask[imu_offset + 3] = ers_side;
par_mask[imu_offset + 4] = ers_vert;
par_mask[imu_offset + 5] = ers_forw;
/*
boolean [] par_mask = {
use_disparity, //sym0
use_aztilts, //sym1
......@@ -1007,6 +1088,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
ers_vert, //sym17
ers_forw //sym18
};
*/
if (manual_par_sel != 0) { // not used in lwir
for (int i = 0; i < par_mask.length; i++) {
par_mask[i] = ((manual_par_sel >> i) & 1) != 0;
......@@ -1046,13 +1128,13 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
boolean [] par_mask)
{
int num_pars = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int npar = 0; npar < getLength(); npar++) if ((par_mask==null) || par_mask[npar]) {
num_pars++;
}
double [][] sym_to_tar= dTar_j_dSym_i();
double [][] jt_part = new double[num_pars][port_coord_deriv.length];
int opar = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int npar = 0; npar < getLength(); npar++) if ((par_mask==null) || par_mask[npar]) {
for (int i = 0; i < port_coord_deriv.length; i++){ // pxy index (0..7)
for (int k = 0; k < sym_to_tar[npar].length; k++){
jt_part[opar][i] += sym_to_tar[npar][k]* port_coord_deriv[i][k]; // transposing port_coord_deriv
......@@ -1080,7 +1162,7 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
}
double [] sym_array = new double [num_pars];
int opar = 0;
for (int npar = 0; npar < LENGTH; npar++) if ((par_mask==null) || par_mask[npar]) {
for (int npar = 0; npar < getLength(); npar++) if ((par_mask==null) || par_mask[npar]) {
for (int i = 0; i < tar_array.length; i++){
sym_array[opar] += tar_array[i] * tar_to_sym[i][npar];
}
......@@ -1095,9 +1177,9 @@ matrix([[-0.125, -0.125, 0.125, 0.125, -0.125, 0.125, -0. , -0. , -0.
{
double [][] sym_to_tar = dTar_j_dSym_i();
double [] tar_array = new double [sym_to_tar[0].length];
for (int npar = 0; npar < LENGTH; npar++) {
for (int npar = 0; npar < getLength(); npar++) {
int spar = 0;
for (int i = 0; i < LENGTH; i++) if ((par_mask==null) || par_mask[i]){
for (int i = 0; i < getLength(); i++) if ((par_mask==null) || par_mask[i]){
tar_array[npar] += sym_array[spar] * sym_to_tar[i][npar];
spar++;
}
......
......@@ -7,6 +7,7 @@ import java.nio.ByteOrder;
import java.nio.channels.Channels;
import java.nio.channels.WritableByteChannel;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Properties;
import com.elphel.imagej.common.GenericJTabbedDialog;
......@@ -52,9 +53,7 @@ public class GeometryCorrection {
// use static CorrVector.getCorrNames(numSensors)
// or non-static corrVector.getCorrNames();
public String [] getCorrNames() {
return CorrVector.getCorrNames(numSensors);
}
/*
static final String [] CORR_NAMES = { // need to be fixed too!
"tilt0","tilt1","tilt2",
"azimuth0","azimuth1","azimuth2",
......@@ -62,7 +61,7 @@ public class GeometryCorrection {
"zoom0","zoom1","zoom2",
"omega_tilt", "omega_azimuth", "omega_roll",
"velocity_x", "velocity_y", "velocity_z"};
*/
public int debugLevel = 0;
public double line_time = 36.38E-6; // 26.5E-6; // duration of sensor scan line (for ERS) Wrong, 36.38us (change and re-run ERS
public int pixelCorrectionWidth=2592; // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2)
......@@ -141,6 +140,49 @@ public class GeometryCorrection {
public int [] woi_tops = null; // used to calculate scanline timing
public int [] camera_heights = null; // actual acquired lines (from woi_tops)
public String [] getCorrNames() {
return CorrVector.getCorrNames(numSensors);
}
// Removing dependence on CORR_NAMES in QuadCLTCPU
public void setPropertiesExtrinsic(String prefix, Properties properties) {
String [] corr_names = getCorrNames(); // variable number of cameras
for (int i = 0; i < corr_names.length; i++){
String name = prefix+"extrinsic_corr_"+corr_names[i];
properties.setProperty(name, getCorrVector().toArray()[i]+"");
}
}
public static void setPropertiesExtrinsic(String prefix, Properties properties, double [] extrinsic_corr) {
int num_cams = CorrVector.getCamerasFromEV(extrinsic_corr.length);
String [] corr_names = CorrVector.getCorrNames(num_cams); // variable number of cameras
for (int i = 0; i < corr_names.length; i++){
String name = prefix+"extrinsic_corr_"+corr_names[i];
if (!Double.isNaN( extrinsic_corr[i])) {
properties.setProperty(name, extrinsic_corr[i]+"");
}
}
}
public static double [] getPropertiesExtrinsic(String prefix, Properties properties) {
// determine number of cameras from maximal tilt?
int num_chn = 4;
for (; true; num_chn++) {
String name = prefix+"extrinsic_corr_"+CorrVector.CORR_TILT+(num_chn - 1); // maximal tilt index = N-2 (tilt2 for quad)
if (properties.getProperty(name) == null) {
break;
}
}
String [] corr_names = CorrVector.getCorrNames(num_chn);
double [] extrinsic_vect = new double [corr_names.length];
Arrays.fill(extrinsic_vect, Double.NaN);
for (int i = 0; i < extrinsic_vect.length; i++) {
String name = prefix+"extrinsic_corr_"+corr_names[i];
if (properties.getProperty(name)!=null) {
extrinsic_vect[i] = Double.parseDouble(properties.getProperty(name));
}
}
return extrinsic_vect;
}
public float [] toFloatArray() { // for GPU comparison
return new float[] {
......
......@@ -110,7 +110,7 @@ public class QuadCLTCPU {
public EyesisCorrectionParameters.CorrectionParameters correctionsParameters=null;
double [][][][][][] clt_kernels = null; // can be used to determine monochrome too?
public GeometryCorrection geometryCorrection = null;
double [] extrinsic_vect = new double [GeometryCorrection.CORR_NAMES.length]; // extrinsic corrections (needed from properties, before geometryCorrection
double[] extrinsic_vect; // = new double [GeometryCorrection.CORR_NAMES.length]; // extrinsic corrections (needed from properties, before geometryCorrection
public int extra_items = 8; // number of extra items saved with kernels (center offset (partial, full, derivatives)
public ImagePlus eyesisKernelImage = null;
public long startTime; // start of batch processing
......@@ -666,10 +666,13 @@ public class QuadCLTCPU {
if (gc == null) { // if it was not yet created
gc = new GeometryCorrection(this.extrinsic_vect); // not used in lwir
}
gc.setPropertiesExtrinsic(prefix, properties);
/*
for (int i = 0; i < GeometryCorrection.CORR_NAMES.length; i++){
String name = prefix+"extrinsic_corr_"+GeometryCorrection.CORR_NAMES[i];
properties.setProperty(name, gc.getCorrVector().toArray()[i]+"");
}
*/
if (is_aux && (gc.rigOffset != null)) {
gc.rigOffset.setProperties(prefix,properties);
}
......@@ -697,10 +700,16 @@ public class QuadCLTCPU {
}
}
}
/*
GeometryCorrection gc = geometryCorrection;
if (gc == null) { // if it was not yet created
gc = new GeometryCorrection(this.extrinsic_vect);
}
*/
double [] other_extrinsic_vect = GeometryCorrection.getPropertiesExtrinsic(other_prefix, other_properties);
int num_cams = CorrVector.getCamerasFromEV(other_extrinsic_vect.length);
GeometryCorrection.setPropertiesExtrinsic(this_prefix, properties, other_extrinsic_vect);
/*
for (int i = 0; i < GeometryCorrection.CORR_NAMES.length; i++){
String other_name = other_prefix+"extrinsic_corr_"+GeometryCorrection.CORR_NAMES[i];
if (other_properties.getProperty(other_name)!=null) {
......@@ -713,6 +722,7 @@ public class QuadCLTCPU {
properties.setProperty(this_name, gc.getCorrVector().toArray()[i]+"");
// System.out.println("copyPropertiesFrom():"+i+": setProperty("+this_name+","+gc.getCorrVector().toArray()[i]+"");
}
*/
// System.out.println("Done copyPropertiesFrom");
}
......@@ -751,6 +761,21 @@ public class QuadCLTCPU {
}
}
// always set extrinsic_corr
double [] new_extrinsic_vect = GeometryCorrection.getPropertiesExtrinsic(prefix, properties);
int num_cams = CorrVector.getCamerasFromEV(new_extrinsic_vect.length);
for (int i = 0; i < new_extrinsic_vect.length; i++) {
if (!Double.isNaN(new_extrinsic_vect[i])) {
if (this.extrinsic_vect == null) { // not used in lwir
// only create non-null array if there are saved values
this.extrinsic_vect = new_extrinsic_vect.clone();
}
this.extrinsic_vect[i] = new_extrinsic_vect[i];
if (geometryCorrection != null){ // not used in lwir
geometryCorrection.setCorrVector(i,this.extrinsic_vect[i]); // should be same mumber of cameras
}
}
}
/*
for (int i = 0; i < GeometryCorrection.CORR_NAMES.length; i++){
String name = prefix+"extrinsic_corr_"+GeometryCorrection.CORR_NAMES[i];
if (properties.getProperty(name)!=null) {
......@@ -759,20 +784,12 @@ public class QuadCLTCPU {
this.extrinsic_vect = new double [GeometryCorrection.CORR_NAMES.length];
}
this.extrinsic_vect[i] = Double.parseDouble(properties.getProperty(name));
// System.out.println("getProperties():"+i+": getProperty("+name+") -> "+properties.getProperty(name)+"");
if (geometryCorrection != null){ // not used in lwir
// if (geometryCorrection.getCorrVector().toArray() == null) {
// geometryCorrection.resetCorrVector(); // make it array of zeros
// }
// geometryCorrection.getCorrVector().toArray()[i] = this.extrinsic_vect[i];
geometryCorrection.setCorrVector(i,this.extrinsic_vect[i]);
}
}
}
// if (is_aux && (geometryCorrection != null)) {
// geometryCorrection.setRigOffsetFromProperies(prefix, properties);
// }
*/
if (geometryCorrection == null) {
double [] extrinsic_vect_saved = this.extrinsic_vect.clone();
......@@ -846,11 +863,11 @@ public class QuadCLTCPU {
// TODO: Verify correction sign!
double f_avg = geometryCorrection.getCorrVector().setZoomsFromF(
sensors[0].focalLength,
sensors[1].focalLength,
sensors[2].focalLength,
sensors[3].focalLength);
double [] f_lengths = new double [sensors.length];
for (int i = 0; i < f_lengths.length; i++) {
f_lengths[i] = sensors[i].focalLength;
}
double f_avg = geometryCorrection.getCorrVector().setZoomsFromF(f_lengths);
// following parameters are used for scaling extrinsic corrections
geometryCorrection.focalLength = f_avg;
......@@ -875,28 +892,13 @@ public class QuadCLTCPU {
sensors[0].pixelCorrectionHeight,
sensors[0].pixelSize);
// set other/individual sensor parameters
/*
for (int i = 1; i < numSensors; i++){
if ( (sensors[0].theta != sensors[i].theta) || // elevation
(sensors[0].heading != sensors[i].heading)){
System.out.println("initGeometryCorrection(): All sensors have to have the same elevation and heading, but channels 0 and "+i+" mismatch");
return false;
}
}
*/
double theta_avg = geometryCorrection.getCorrVector().setTiltsFromThetas(
sensors[0].theta,
sensors[1].theta,
sensors[2].theta,
sensors[3].theta);
double heading_avg = geometryCorrection.getCorrVector().setAzimuthsFromHeadings(
sensors[0].heading,
sensors[1].heading,
sensors[2].heading,
sensors[3].heading);
double [] thetas = new double [sensors.length];
for (int i = 0; i < thetas.length; i++) thetas[i] = sensors[i].theta;
double theta_avg = geometryCorrection.getCorrVector().setTiltsFromThetas(thetas);
double [] headings = new double [sensors.length];
for (int i = 0; i < headings.length; i++) headings[i] = sensors[i].heading;
double heading_avg = geometryCorrection.getCorrVector().setAzimuthsFromHeadings(headings);
double [] forward = new double[numSensors];
double [] right = new double[numSensors];
double [] height = new double[numSensors];
......
package com.elphel.imagej.tileprocessor;
import java.util.ArrayList;
import java.util.HashMap;
/**
**
......@@ -62,10 +63,47 @@ public class SymmVector {
private int num_rz_defined; // number of defined rz_indices
private boolean [] used_rz_indices;// already used rz_vectors
private double [] cumul_rz_influences; // will not work? - all candidates correlate the same
public int debug_level = -1;
// caching results of vectors generation for used camera configuration not to re-generate each time
// CorrVector is constructed
public static HashMap <Integer,SymmVectorsSet> vectors_cache = new HashMap <Integer,SymmVectorsSet>();
public static SymmVectorsSet getSymmVectorsSet (int num_cameras) {
SymmVectorsSet rvs = vectors_cache.get(num_cameras);
if (rvs == null) {
rvs = newVectors (num_cameras);
vectors_cache.put(num_cameras, rvs);
}
return rvs;
}
public int debug_level = -1;
public static double [][] getSymmXY(int num_cameras){
SymmVectorsSet rvs = vectors_cache.get(num_cameras);
if (rvs == null) {
rvs = newVectors (num_cameras);
vectors_cache.put(num_cameras, rvs);
}
return rvs.xy;
}
public static SymmVectorsSet newVectors (int num_cameras) {
boolean full_type1 = false;
boolean full_type2 = false;
int debug_level = -1;
SymmVectorsSet rvs = new SymmVectorsSet();
SymmVector sv = new SymmVector(
num_cameras,
full_type1,
full_type2,
debug_level);
rvs.xy = sv.exportXY();
rvs.rt = sv.exportRT();
rvs.dir_rt = sv.exportDirRT();
rvs.rots = sv.exportRZ(false); // include common roll
rvs.zooms = sv.exportRZ(true); // no common zoom
return rvs;
}
public SymmVector (
int num_cameras,
......@@ -343,6 +381,18 @@ public class SymmVector {
return rslt;
}
/**
* Export directions (0 - radial to center, 1 - tangential CW, 2 - radial out , 3 tangential CW
* @return
*/
public int [][] exportDirRT(){
int [][] rslt = new int[sym_indices.length][];
for (int n = 0; n < sym_indices.length; n++){
rslt[n]= proto_all[sym_indices[n]];
}
return rslt;
}
/**
*
* @param zoom_mode
......@@ -1064,3 +1114,12 @@ public class SymmVector {
*/
}
class SymmVectorsSet {
// int num_sensors;
double [][] xy;
double [][] rt;
int [][] dir_rt;
double [][] rots;
double [][] zooms;
}
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