Skip to content

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

working on quaterions <-> affines

parent 7ad0875e
Show whitespace changes
Inline Side-by-side
Showing with 808 additions and 50 deletions
src/main/java/com/elphel/imagej/orthomosaic/OrthoAltitudeMatch.java
View file @ 6a6d0c40
...@@ -54,6 +54,7 @@ public class OrthoAltitudeMatch { ...@@ -54,6 +54,7 @@ public class OrthoAltitudeMatch {
String log_path, String log_path,
String orthoMapsCollection_path, String orthoMapsCollection_path,
int debugLevel) { int debugLevel) {
boolean y_down_ccw = true;
int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs int [] indices = orthoMapsCollection.getScenesFromPairs( // may be shorter, each element - absolute scene number used in pairs
available_pairs, // pairs_defined_abs,// int [][] pairs, available_pairs, // pairs_defined_abs,// int [][] pairs,
null); // int [] indices_in) // preselected indices or null null); // int [] indices_in) // preselected indices or null
...@@ -128,30 +129,6 @@ public class OrthoAltitudeMatch { ...@@ -128,30 +129,6 @@ public class OrthoAltitudeMatch {
if (updateStatus) { if (updateStatus) {
IJ.showStatus("Processing pair "+npair+" of "+condensed_pairs.length+" "+ipair[0]+"->"+ipair[1]); IJ.showStatus("Processing pair "+npair+" of "+condensed_pairs.length+" "+ipair[0]+"->"+ipair[1]);
} }
/*
PairwiseOrthoMatch pairwiseOrthoMatch = ortho_maps[ipair[0]].getMatch(ortho_maps[ipair[1]].getName(), true).clone(); // ?
double [][] daffine = null;
if (pairwiseOrthoMatch != null) {
double [] enuOffset = ortho_maps[ipair[1]].enuOffsetTo(ortho_maps[ipair[0]]);
double [] rd = {enuOffset[0], -enuOffset[1]}; // {right,down} of the image
daffine = pairwiseOrthoMatch.getAffine();
if ((daffine != null) && alt_pairwise) {
} else { // combine differential affine from individual
double [][] aff0 = ortho_maps[ipair[0]].getAffine();
double [][] aff1 = ortho_maps[ipair[1]].getAffine();
PairwiseOrthoMatch aff_match = new PairwiseOrthoMatch (
aff0, // double [][] affine0,
aff1, // double [][] affine1,
rd); // double [] rd);
daffine = aff_match.getAffine();
pairwiseOrthoMatch.setAffine(daffine);
}
} else {
System.out.println("BUG: Missing pair for ["+ipair[0]+", "+ipair[1]+"] ");
continue;
}
*/
PairwiseOrthoMatch pairwiseOrthoMatch = ortho_maps[ipair[0]].getMatch(ortho_maps[ipair[1]].getName(), true); // ? PairwiseOrthoMatch pairwiseOrthoMatch = ortho_maps[ipair[0]].getMatch(ortho_maps[ipair[1]].getName(), true); // ?
if (pairwiseOrthoMatch == null) { if (pairwiseOrthoMatch == null) {
System.out.println("BUG: Missing pair for ["+ipair[0]+", "+ipair[1]+"] "); System.out.println("BUG: Missing pair for ["+ipair[0]+", "+ipair[1]+"] ");
...@@ -179,7 +156,7 @@ public class OrthoAltitudeMatch { ...@@ -179,7 +156,7 @@ public class OrthoAltitudeMatch {
Rectangle woi_overlap = OrthoMap.getDefinedBounds( Rectangle woi_overlap = OrthoMap.getDefinedBounds(
alt_slices, // final double [][] data_slices, alt_slices, // final double [][] data_slices,
width); // final int width); width); // final int width);
double [] diff_data = OrthoMap. subtractWoi( double [] diff_data = OrthoMap.subtractWoi(
alt_multi[cpair[0]], // final double [] data0, alt_multi[cpair[0]], // final double [] data0,
alt_multi[cpair[1]], // final double [] data1, alt_multi[cpair[1]], // final double [] data1,
width, // final int width, width, // final int width,
...@@ -217,11 +194,218 @@ public class OrthoAltitudeMatch { ...@@ -217,11 +194,218 @@ public class OrthoAltitudeMatch {
break; break;
} }
} }
double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
boolean test_quat = true;
if (test_quat) {
System.out.println(">>>>>>>>>>>>>>>>> npair="+npair+": "+ipair[0]+" -> "+ipair[1]);
boolean [][] masks = new boolean[2][];
double [][] alt_data5s = new double[2][];
double [][] data_overlap = new double[2][];
double [][] alt_datas = new double[3][];
double [] quat_diff = QuatUtils.tiltToQuaternion(
alt_data,
y_down_ccw); // boolean y_down_ccw)
double [][] quats01 = new double [alt_datas.length][];
for (int ns = 0; ns < cpair.length; ns++) {
data_overlap[ns] = OrthoMap.extractWoi(
alt_multi[cpair[ns]], // final double [] data0,
width, // final int width,
woi_overlap); // Rectangle woi_in);
for (int ntry = 0; ntry <= alt_refine; ntry++) {
alt_data5s[ns] = OrthoMap.getPlane(
data_overlap[ns], // final double [] data,
masks[ns], // final boolean [] mask,
weight, // final double [] weight,
woi_overlap.width, // final int width,
xy0); // final double [] xy0) {
if ((alt_outliers > 0) && (ntry < alt_refine)){ // not the last pass
masks[ns] = OrthoMap.removeRelativeLowHigh (
data_overlap[ns], // final double [] data,
null, // mask, // final boolean [] mask_in, // new mask for all data and latest plane
alt_abs_outliers, // final double abs_diff,
alt_outliers, // final double rel_frac,
alt_data5s[ns], // final double [] ground_plane, // tiltx,tilty, offs, x0(pix), y0(pix) or null
woi_overlap.width,// final int width, // only used with ground_plane != null;
num_bins); // final int num_bins)
} else {
break;
}
}
alt_datas[ns] = new double [] {alt_data5s[ns][0]/pix_size_meters, alt_data5s[ns][1]/pix_size_meters,alt_data5s[ns][2]};
}
alt_datas[2] = new double [3];
for (int i = 0; i < alt_datas[2].length; i++) {
alt_datas[2][i] = alt_datas[0][i]+alt_data[i];
}
for (int ns = 0; ns < quats01.length; ns++) {
quats01[ns] = QuatUtils.tiltToQuaternion(
alt_datas[ns],
y_down_ccw); // boolean y_down_ccw)
}
double [] quat_rdiff = QuatUtils.invert(quat_diff);
double [] quat2a = QuatUtils.multiply(quats01[0], quat_diff);
double [] quat2b = QuatUtils.multiply(quat_diff, quats01[0]);
double [] quat2ap = QuatUtils.pureTilt(quat2a);
double [] quat2bp = QuatUtils.pureTilt(quat2b);
double [] quat2an = QuatUtils.normalize(quat2a);
double [] quat2bn = QuatUtils.normalize(quat2b);
double [] quat2apn = QuatUtils.normalize(quat2ap);
double [] quat2bpn = QuatUtils.normalize(quat2bp);
double [] quat1ar = QuatUtils.multiply(quats01[1], quat_rdiff);
double [] quat1br = QuatUtils.multiply(quat_rdiff, quats01[1]);
double [] quat1cr = QuatUtils.multiply(quats01[2], quat_rdiff);
double [] quat1dr = QuatUtils.multiply(quat_rdiff, quats01[2]);
double [] quat1apr = QuatUtils.pureTilt(quat1ar);
double [] quat1bpr = QuatUtils.pureTilt(quat1br);
double [] quat1cpr = QuatUtils.pureTilt(quat1cr);
double [] quat1dpr = QuatUtils.pureTilt(quat1dr);
double [] quat1aprn = QuatUtils.normalize(quat1apr);
double [] quat1bprn = QuatUtils.normalize(quat1bpr);
double [] quat1cprn = QuatUtils.normalize(quat1cpr);
double [] quat1dprn = QuatUtils.normalize(quat1dpr);
double [] quat1arn = QuatUtils.normalize(quat1ar);
double [] quat1brn = QuatUtils.normalize(quat1br);
double [] quat1crn = QuatUtils.normalize(quat1cr);
double [] quat1drn = QuatUtils.normalize(quat1dr);
//pureTilt
System.out.println("alt_data= ["+alt_data[0]+","+alt_data[1]+","+alt_data[2]+"]");
System.out.println("alt_datas[0]=["+alt_datas[0][0]+","+alt_datas[0][1]+","+alt_datas[0][2]+"]");
System.out.println("alt_datas[1]=["+alt_datas[1][0]+","+alt_datas[1][1]+","+alt_datas[1][2]+"]");
System.out.println("alt_datas[2]=["+alt_datas[2][0]+","+alt_datas[2][1]+","+alt_datas[2][2]+"]");
/*
System.out.println("quat_diff= ["+quat_diff[0]+","+quat_diff[1]+","+quat_diff[2]+","+quat_diff[3]+"] "+QuatUtils.norm(quat_diff));
System.out.println("quats01[0]= ["+quats01[0][0]+","+quats01[0][1]+","+quats01[0][2]+","+quats01[0][3]+"] "+QuatUtils.norm(quats01[0]));
System.out.println("quats01[1]= ["+quats01[1][0]+","+quats01[1][1]+","+quats01[1][2]+","+quats01[1][3]+"] "+QuatUtils.norm(quats01[1]));
System.out.println("quats01[2]= ["+quats01[2][0]+","+quats01[2][1]+","+quats01[2][2]+","+quats01[2][3]+"] "+QuatUtils.norm(quats01[2]));
*/
System.out.println("quat2a= ["+quat2a[0]+","+quat2a[1]+","+quat2a[2]+","+quat2a[3]+"] "+QuatUtils.norm(quat2a));
System.out.println("quat2b= ["+quat2b[0]+","+quat2b[1]+","+quat2b[2]+","+quat2b[3]+"] "+QuatUtils.norm(quat2b));
System.out.println("quat2ap= ["+quat2ap[0]+","+quat2ap[1]+","+quat2ap[2]+","+quat2ap[3]+"] "+QuatUtils.norm(quat2ap));
System.out.println("quat2bp= ["+quat2bp[0]+","+quat2bp[1]+","+quat2bp[2]+","+quat2bp[3]+"] "+QuatUtils.norm(quat2bp));
/*
System.out.println("quat2an= ["+quat2an[0]+","+quat2an[1]+","+quat2an[2]+","+quat2an[3]+"] "+QuatUtils.norm(quat2an));
System.out.println("quat2bn= ["+quat2bn[0]+","+quat2bn[1]+","+quat2bn[2]+","+quat2bn[3]+"] "+QuatUtils.norm(quat2bn));
System.out.println("quat2apn= ["+quat2apn[0]+","+quat2apn[1]+","+quat2apn[2]+","+quat2apn[3]+"] "+QuatUtils.norm(quat2apn));
System.out.println("quat2bpn= ["+quat2bpn[0]+","+quat2bpn[1]+","+quat2bpn[2]+","+quat2bpn[3]+"] "+QuatUtils.norm(quat2bpn));
System.out.println("quat1apr= ["+quat1apr[0]+","+quat1apr[1]+","+quat1apr[2]+","+quat1apr[3]+"] "+QuatUtils.norm(quat1apr)); // +
System.out.println("quat1bpr= ["+quat1bpr[0]+","+quat1bpr[1]+","+quat1bpr[2]+","+quat1bpr[3]+"] "+QuatUtils.norm(quat1bpr)); // ++
System.out.println("quat1cpr= ["+quat1cpr[0]+","+quat1cpr[1]+","+quat1cpr[2]+","+quat1cpr[3]+"] "+QuatUtils.norm(quat1cpr)); // +++
System.out.println("quat1dpr= ["+quat1dpr[0]+","+quat1dpr[1]+","+quat1dpr[2]+","+quat1dpr[3]+"] "+QuatUtils.norm(quat1dpr)); // +++
System.out.println("quat1aprn= ["+quat1aprn[0]+","+quat1aprn[1]+","+quat1aprn[2]+","+quat1aprn[3]+"] "+QuatUtils.norm(quat1aprn)); // +
System.out.println("quat1bprn= ["+quat1bprn[0]+","+quat1bprn[1]+","+quat1bprn[2]+","+quat1bprn[3]+"] "+QuatUtils.norm(quat1bprn)); // ++
System.out.println("quat1cprn= ["+quat1cprn[0]+","+quat1cprn[1]+","+quat1cprn[2]+","+quat1cprn[3]+"] "+QuatUtils.norm(quat1cprn)); // +++
System.out.println("quat1dprn= ["+quat1dprn[0]+","+quat1dprn[1]+","+quat1dprn[2]+","+quat1dprn[3]+"] "+QuatUtils.norm(quat1dprn)); // +++
System.out.println("quat1arn= ["+quat1arn[0]+","+quat1arn[1]+","+quat1arn[2]+","+quat1arn[3]+"] "+QuatUtils.norm(quat1arn)); // +
System.out.println("quat1brn= ["+quat1brn[0]+","+quat1brn[1]+","+quat1brn[2]+","+quat1brn[3]+"] "+QuatUtils.norm(quat1brn)); // ++
System.out.println("quat1crn= ["+quat1crn[0]+","+quat1crn[1]+","+quat1crn[2]+","+quat1crn[3]+"] "+QuatUtils.norm(quat1crn)); // +++
System.out.println("quat1drn= ["+quat1drn[0]+","+quat1drn[1]+","+quat1drn[2]+","+quat1drn[3]+"] "+QuatUtils.norm(quat1drn)); // +++
*/
double [][] affine_pair = pairwiseOrthoMatch.getAffine();
double [][][] affines = new double [][][] {ortho_maps[ipair[0]].getAffine(),ortho_maps[ipair[1]].getAffine()};
boolean make__pure_tilt = false;
double [][] aff1_stretch = QuatUtils.quatToAffine(
quats01[0], // double [] quat,
true, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] aff2_shrink = QuatUtils.quatToAffine(
quats01[2], // double [] quat,
false, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] aff2_stretch = QuatUtils.quatToAffine(
quats01[2], // double [] quat,
true, // boolean stretch,
make__pure_tilt, // boolean make__pure_tilt)
y_down_ccw); // boolean y_down_ccw);
double [][] qaffd_pm = QuatUtils.affineToQuatScaled(affine_pair, y_down_ccw);
double [] sqaffd_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaffd_pm[1])}; // normalizes
System.out.println("qaffd_pm[0]= ["+qaffd_pm[0][0]+ ","+qaffd_pm[0][1]+ ","+qaffd_pm[0][2]+ ","+qaffd_pm[0][3]+ "] scale="+sqaffd_pm[0]);
System.out.println("qaffd_pm[1]= ["+qaffd_pm[1][0]+ ","+qaffd_pm[1][1]+ ","+qaffd_pm[1][2]+ ","+qaffd_pm[1][3]+ "] scale="+sqaffd_pm[1]);
double [][] qaff1_pm = QuatUtils.affineToQuatScaled(aff1_stretch, y_down_ccw);
double [] sqaff1_pm = {QuatUtils.normalizeInPlace(qaffd_pm[0]),QuatUtils.normalizeInPlace(qaff1_pm[1])}; // normalizes
System.out.println("qaff1_pm[0]= ["+qaff1_pm[0][0]+ ","+qaff1_pm[0][1]+ ","+qaff1_pm[0][2]+ ","+qaff1_pm[0][3]+ "] scale="+sqaff1_pm[0]);
System.out.println("qaff1_pm[1]= ["+qaff1_pm[1][0]+ ","+qaff1_pm[1][1]+ ","+qaff1_pm[1][2]+ ","+qaff1_pm[1][3]+ "] scale="+sqaff1_pm[1]);
double [][] qaff2a_pm = QuatUtils.affineToQuatScaled(aff2_shrink, y_down_ccw);
double [] sqaff2a_pm = {QuatUtils.normalizeInPlace(qaff2a_pm[0]),QuatUtils.normalizeInPlace(qaff2a_pm[1])}; // normalizes
System.out.println("qaff2a_pm[0]= ["+qaff2a_pm[0][0]+","+qaff2a_pm[0][1]+","+qaff2a_pm[0][2]+","+qaff2a_pm[0][3]+"] scale="+sqaff2a_pm[0]);
System.out.println("qaff2a_pm[1]= ["+qaff2a_pm[1][0]+","+qaff2a_pm[1][1]+","+qaff2a_pm[1][2]+","+qaff2a_pm[1][3]+"] scale="+sqaff2a_pm[1]);
double [][] qaff2_pm = QuatUtils.affineToQuatScaled(aff2_stretch, y_down_ccw);
double [] sqaff2_pm = {QuatUtils.normalizeInPlace(qaff2_pm[0]),QuatUtils.normalizeInPlace(qaff2_pm[1])}; // normalizes
System.out.println("qaff2_pm[0]= ["+qaff2_pm[0][0]+ ","+qaff2_pm[0][1]+ ","+qaff2_pm[0][2]+ ","+qaff2_pm[0][3]+ "] scale="+sqaff2_pm[0]);
System.out.println("qaff2_pm[1]= ["+qaff2_pm[1][0]+ ","+qaff2_pm[1][1]+ ","+qaff2_pm[1][2]+ ","+qaff2_pm[1][3]+ "] scale="+sqaff2_pm[1]);
double [][] qaffine0_pm = QuatUtils.affineToQuatScaled(affines[0], y_down_ccw);
double [] sqaffine0_pm ={QuatUtils.normalizeInPlace(qaffine0_pm[0]),QuatUtils.normalizeInPlace(qaffine0_pm[1])}; // normalizes
System.out.println("qaffine0_pm[0]=["+qaffine0_pm[0][0]+ ","+qaffine0_pm[0][1]+ ","+qaffine0_pm[0][2]+ ","+qaffine0_pm[0][3]+ "] scale="+sqaffine0_pm[0]);
System.out.println("qaffine0_pm[1]=["+qaffine0_pm[1][0]+ ","+qaffine0_pm[1][1]+ ","+qaffine0_pm[1][2]+ ","+qaffine0_pm[1][3]+ "] scale="+sqaffine0_pm[1]);
double [][] qaffine1_pm = QuatUtils.affineToQuatScaled(affines[1], y_down_ccw);
double [] sqaffine1_pm ={QuatUtils.normalizeInPlace(qaffine1_pm[0]),QuatUtils.normalizeInPlace(qaffine1_pm[1])}; // normalizes
System.out.println("qaffine1_pm[0]=["+qaffine1_pm[0][0]+ ","+qaffine1_pm[0][1]+ ","+qaffine1_pm[0][2]+ ","+qaffine1_pm[0][3]+ "] scale="+sqaffine1_pm[0]);
System.out.println("qaffine1_pm[1]=["+qaffine1_pm[1][0]+ ","+qaffine1_pm[1][1]+ ","+qaffine1_pm[1][2]+ ","+qaffine1_pm[1][3]+ "] scale="+sqaffine1_pm[1]);
double [][] aff_combo = QuatUtils.matMult(aff2_shrink,aff1_stretch);
System.out.println("affine_pair= [["+affine_pair[0][0]+","+affine_pair[0][1]+","+affine_pair[0][2]+"]");
System.out.println(" ["+affine_pair[1][0]+","+affine_pair[1][1]+","+affine_pair[1][2]+"]]");
System.out.println("affines[0]= [["+affines[0][0][0]+ ","+affines[0][0][1]+ ","+affines[0][0][2]+"]");
System.out.println(" ["+affines[0][1][0]+ ","+affines[0][1][1]+ ","+affines[0][1][2]+"]]");
System.out.println("affines[1]= [["+affines[1][0][0]+ ","+affines[1][0][1]+ ","+affines[1][0][2]+"]");
System.out.println(" ["+affines[1][1][0]+ ","+affines[1][1][1]+ ","+affines[1][1][2]+"]]");
System.out.println("aff1_stretch= [["+aff1_stretch[0][0]+ ","+aff1_stretch[0][1]+"]");
System.out.println(" ["+aff1_stretch[1][0]+ ","+aff1_stretch[1][1]+"]]");
System.out.println("aff2_shrink= [["+aff2_shrink[0][0]+ ","+aff2_shrink[0][1]+ "]");
System.out.println(" ["+aff2_shrink[1][0]+ ","+aff2_shrink[1][1]+ "]]");
System.out.println("aff2_stretch= [["+aff2_stretch[0][0]+ ","+aff2_stretch[0][1]+"]");
System.out.println(" ["+aff2_stretch[1][0]+ ","+aff2_stretch[1][1]+"]]");
System.out.println("aff_combo= [["+aff_combo[0][0]+ ","+aff_combo[0][1]+ "]");
System.out.println(" ["+aff_combo[1][0]+ ","+aff_combo[1][1]+ "]]");
/*
double [] svd_affine_pair = OrthoMap.singularValueDecomposeScaleTilt(affine_pair, y_down_ccw); // boolean y_down_ccw)
double [][] svd_affines = {OrthoMap.singularValueDecomposeScaleTilt(affines[0], y_down_ccw), OrthoMap.singularValueDecomposeScaleTilt(affines[1], y_down_ccw)};
double [] svd_aff1_stretch = OrthoMap.singularValueDecomposeScaleTilt(aff1_stretch, y_down_ccw);
double [] svd_aff2_shrink = OrthoMap.singularValueDecomposeScaleTilt(aff2_shrink, y_down_ccw);
double [] svd_aff2_stretch = OrthoMap.singularValueDecomposeScaleTilt(aff2_stretch, y_down_ccw);
double [] svd_aff_combo = OrthoMap.singularValueDecomposeScaleTilt(aff_combo, y_down_ccw);
*/
SingularValueDecomposition svd_affine_pair = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affine_pair, y_down_ccw); // boolean y_down_ccw)
SingularValueDecomposition[] svd_affines = {SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[0], y_down_ccw), SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(affines[1], y_down_ccw)};
SingularValueDecomposition svd_aff1_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff1_stretch, y_down_ccw);
SingularValueDecomposition svd_aff2_shrink = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_shrink, y_down_ccw);
SingularValueDecomposition svd_aff2_stretch = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff2_stretch, y_down_ccw);
SingularValueDecomposition svd_aff_combo = SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(aff_combo, y_down_ccw);
System.out.println("scale,tilt_rad,gamma,rot");
System.out.println("svd_affine_pair= ["+svd_affine_pair.scale+ ","+svd_affine_pair.getTiltAngle()+ ","+svd_affine_pair.gamma+ ","+svd_affine_pair.rot+ "] tilt="+(svd_affine_pair.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_affine_pair.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_affines[0]= ["+svd_affines[0].scale+ ","+ svd_affines[0].getTiltAngle()+ ","+svd_affines[0].gamma+ ","+svd_affines[0].rot+ "] tilt="+(svd_affines[0].getTiltAngle()* 180/Math.PI)+ "\u00B0, dir="+(svd_affines[0].gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_affines[1]= ["+svd_affines[1].scale+ ","+ svd_affines[1].getTiltAngle()+ ","+svd_affines[1].gamma+ ","+svd_affines[1].rot+ "] tilt="+(svd_affines[1].getTiltAngle()* 180/Math.PI)+ "\u00B0, dir="+(svd_affines[1].gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff1_stretch= ["+svd_aff1_stretch.scale+","+ svd_aff1_stretch.getTiltAngle()+","+svd_aff1_stretch.gamma+","+svd_aff1_stretch.rot+"] tilt="+(svd_aff1_stretch.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff1_stretch.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff2_shrink= ["+svd_aff2_shrink.scale+ ","+ svd_aff2_shrink.getTiltAngle()+ ","+svd_aff2_shrink.gamma+ ","+svd_aff2_shrink.rot+ "] tilt="+(svd_aff2_shrink.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff2_shrink.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff2_stretch= ["+svd_aff2_stretch.scale+","+ svd_aff2_stretch.getTiltAngle()+","+svd_aff2_stretch.gamma+","+svd_aff2_stretch.rot+"] tilt="+(svd_aff2_stretch.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff2_stretch.gamma*180/Math.PI)+"\u00B0");
System.out.println("svd_aff_combo= ["+svd_aff_combo.scale+ ","+ svd_aff_combo.getTiltAngle()+ ","+svd_aff_combo.gamma+ ","+svd_aff_combo.rot+ "] tilt="+(svd_aff_combo.getTiltAngle()*180/Math.PI)+ "\u00B0, dir="+(svd_aff_combo.gamma*180/Math.PI)+"\u00B0");
System.out.println("quat_diff= ["+quat_diff[0]+ ","+quat_diff[1]+ ","+quat_diff[2]+ ","+quat_diff[3]+ "] tilt="+2*Math.acos(quat_diff[0])+ " dir="+Math.atan2(quat_diff[2],quat_diff[1])+", tilt="+(2*Math.acos(quat_diff[0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quat_diff[2],quat_diff[1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[0]= ["+quats01[0][0]+","+quats01[0][1]+","+quats01[0][2]+","+quats01[0][3]+"] tilt="+2*Math.acos(quats01[0][0])+" dir="+Math.atan2(quats01[0][2],quats01[0][1])+", tilt="+(2*Math.acos(quats01[0][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[0][2],quats01[0][1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[1]= ["+quats01[1][0]+","+quats01[1][1]+","+quats01[1][2]+","+quats01[1][3]+"] tilt="+2*Math.acos(quats01[1][0])+" dir="+Math.atan2(quats01[1][2],quats01[1][1])+", tilt="+(2*Math.acos(quats01[1][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[1][2],quats01[1][1])*180/Math.PI)+"\u00B0");
System.out.println("quats01[2]= ["+quats01[2][0]+","+quats01[2][1]+","+quats01[2][2]+","+quats01[2][3]+"] tilt="+2*Math.acos(quats01[2][0])+" dir="+Math.atan2(quats01[2][2],quats01[2][1])+", tilt="+(2*Math.acos(quats01[2][0])*180/Math.PI)+ "\u00B0, dir="+(Math.atan2(quats01[2][2],quats01[2][1])*180/Math.PI)+"\u00B0");
/*
System.out.println("quat_diff_tilt= "+2*Math.acos(quat_diff[0]));
System.out.println("quats01_0_tilt= "+2*Math.acos(quats01[0][0]));
System.out.println("quats01_1_tilt= "+2*Math.acos(quats01[1][0]));
System.out.println("quats01_2_tilt= "+2*Math.acos(quats01[2][0]));
*/
}
// double [] alt_data = new double[3]; // double [] alt_data = new double[3];
// System.arraycopy(alt_data5, 0, alt_data, 0, alt_data.length); // System.arraycopy(alt_data5, 0, alt_data, 0, alt_data.length);
double [] alt_data = {alt_data5[0]/pix_size_meters, alt_data5[1]/pix_size_meters,alt_data5[2]};
pairwiseOrthoMatch.setAltData(alt_data); pairwiseOrthoMatch.setAltData(alt_data);
if (log_append && (log_path != null)) { // assuming directory exists if (log_append && (log_path != null)) { // assuming directory exists
......
src/main/java/com/elphel/imagej/orthomosaic/OrthoMap.java
View file @ 6a6d0c40
/**
** OrthoMap - Dealing with orthographic maps
**
** Copyright (C) 2024 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** OrthoMap.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.orthomosaic; package com.elphel.imagej.orthomosaic;
import java.awt.Color; import java.awt.Color;
...@@ -9,23 +32,15 @@ import java.io.IOException; ...@@ -9,23 +32,15 @@ import java.io.IOException;
import java.io.ObjectInputStream; import java.io.ObjectInputStream;
import java.io.ObjectOutputStream; import java.io.ObjectOutputStream;
import java.io.Serializable; import java.io.Serializable;
import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.text.SimpleDateFormat;
import java.time.LocalDateTime; import java.time.LocalDateTime;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Calendar;
import java.util.Collections; import java.util.Collections;
import java.util.Comparator; import java.util.Comparator;
import java.util.HashMap; import java.util.HashMap;
import java.util.List;
import java.util.Properties; import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.DoubleFHT; import com.elphel.imagej.common.DoubleFHT;
import com.elphel.imagej.common.DoubleGaussianBlur; import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.GenericJTabbedDialog; import com.elphel.imagej.common.GenericJTabbedDialog;
...@@ -98,9 +113,9 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -98,9 +113,9 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
public LocalDateTime dt; public LocalDateTime dt;
// affine convert (input) rectified coordinates (meters) relative to vert_meters to source image // affine convert (input) rectified coordinates (meters) relative to vert_meters to source image
// coordinates relative to vert_meters // coordinates relative to vert_meters
public double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[] public double [][] affine = new double[][] {{1,0,0},{0,1,0}}; // relative to vert_meters[], positive Y is down (as in images)
public double orig_pix_meters; public double orig_pix_meters;
public double [] vert_meters; // offset of the image vertical in meters (scale-invariant) public double [] vert_meters; // offset of the image vertical in meters (scale-invariant), right (X) and down (Y)
public int orig_width; public int orig_width;
public int orig_height; public int orig_height;
public transient FloatImageData orig_image; public transient FloatImageData orig_image;
...@@ -704,7 +719,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -704,7 +719,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
} }
/** /**
* Get vertical point offset from the top-left corner of the original orthoimage in meters * Get vertical point offset from the top-left corner of the original orthoimage in meters (right and down)
* @return vertical point X,Y offset in meters from the top-left image corner * @return vertical point X,Y offset in meters from the top-left image corner
*/ */
public double [] getVertMeters() { public double [] getVertMeters() {
...@@ -3702,9 +3717,6 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -3702,9 +3717,6 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
} }
/** /**
* Fit a plane to the input data, relative to the specified point in Rectangle wnd * Fit a plane to the input data, relative to the specified point in Rectangle wnd
* @param data double input data array * @param data double input data array
...@@ -5508,7 +5520,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -5508,7 +5520,7 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
* R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} , * R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} ,
* non-uniform scale transform W={{w1,0}{0,w2}}, and rotation * non-uniform scale transform W={{w1,0}{0,w2}}, and rotation
* R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}} * R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}}
* A=R1*W*R2 using singular value decomposition * A=R1*W*R2 using singular value decomposition -- tested
* https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation * https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation
* @param A - input 2x2 matrix * @param A - input 2x2 matrix
* @return {s,beta,w,gamma,beta+gamma} * @return {s,beta,w,gamma,beta+gamma}
...@@ -5546,12 +5558,57 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -5546,12 +5558,57 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
* @param A - linear transformation matrix from rectified ground coordinates * @param A - linear transformation matrix from rectified ground coordinates
* to source image coordinates. OK to use 2x3 affine matrix,extra * to source image coordinates. OK to use 2x3 affine matrix,extra
* components will be ignored. * components will be ignored.
* @return {beta, s, t, beta+gamma}, beta+gamma - total rotation * @param y_down_ccw - positive Y is down, positive angles are CCW
* @return {gamma, s, t, beta+gamma}, beta+gamma - total rotation
*/ */
public static double [] singularValueDecomposeScaleTilt( public static double [] singularValueDecomposeScaleTilt(
double [][] A) { double [][] A,
boolean y_down_ccw) {
double [] svd=singularValueDecompose(A);
double w1 = svd[1], w2 = svd[2], g_p_b=svd[4]; // pure rotation // , beta=svd[0]
double gamma = svd[3];
// For Y-down, angles are CW positive, for Y-up angles are CCW positive
// Affines are Y-up?
// considering tilt in y direction (unless long_axis), it should have higher scale
// (and source image coordinates), while X should correspond to
// the axis of rotation, and scale is just scale caused by error in
// altitude.
double s = Math.min(w1, w2);
double t = w1/w2; // <=1.0, ==cos(tilt)
if (w1 > w2) { // rotate tilt by PI/2
t = w2/w1;
gamma += Math.PI/2; // start with rotation (last in matrices)
if (gamma > Math.PI) {
gamma -= 2* Math.PI;
}
}
if (gamma > Math.PI/2) {
gamma -= Math.PI;
} else if (gamma < -Math.PI) {
gamma += Math.PI;
}
if (y_down_ccw) {
gamma =-gamma;
g_p_b = -g_p_b; // pure rotation
}
return new double [] {gamma, s,t, g_p_b};
}
public static double [] singularValueDecomposeScaleTiltOld(
double [][] A,
boolean y_down_ccw) {
double [] svd=singularValueDecompose(A); double [] svd=singularValueDecompose(A);
double w1 = svd[1], w2 = svd[2], beta=svd[0], g_p_b=svd[4]; double w1 = svd[1], w2 = svd[2], beta=svd[0], g_p_b=svd[4];
// For Y-down, angles are CW positive, for Y-up angles are CCW positive
// Affines are Y-up?
if (y_down_ccw) {
beta =-beta; // find where is the error that beta is negated, check rotation
g_p_b = -g_p_b;
}
// considering tilt in y direction, it should have higher scale // considering tilt in y direction, it should have higher scale
// (and source image coordinates), while X should correspond to // (and source image coordinates), while X should correspond to
...@@ -5566,6 +5623,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -5566,6 +5623,12 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
beta -= 2* Math.PI; beta -= 2* Math.PI;
} }
} }
// beta should be in the range of +/-pi/2
if (beta > Math.PI/2) {
beta -= Math.PI;
} else if (beta < -Math.PI) {
beta += Math.PI;
}
return new double [] {beta, s,t, g_p_b}; return new double [] {beta, s,t, g_p_b};
} }
...@@ -5648,7 +5711,36 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{ ...@@ -5648,7 +5711,36 @@ public class OrthoMap implements Comparable <OrthoMap>, Serializable{
} }
ImageDtt.startAndJoin(threads); ImageDtt.startAndJoin(threads);
return data_out; return data_out;
}
public static double [] extractWoi(
final double [] data0,
final int width,
Rectangle woi_in) {
final int height = data0.length/width;
if (woi_in == null) {
woi_in =new Rectangle(0,0,width,height);
}
final Rectangle woi = new Rectangle(woi_in);
final int woi_len = woi.width*woi.height;
final double [] data_out =new double [woi_len];
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iPix = ai.getAndIncrement(); iPix < woi_len; iPix = ai.getAndIncrement()) {
int x = woi.x + (iPix % woi.width);
int y = woi.y + (iPix / woi.width);
int nPix = y * width + x;
data_out[iPix] = data0[nPix];
}
} }
};
}
ImageDtt.startAndJoin(threads);
return data_out;
}
} }
src/main/java/com/elphel/imagej/orthomosaic/OrthoMapsCollection.java
View file @ 6a6d0c40
...@@ -1040,7 +1040,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -1040,7 +1040,7 @@ public class OrthoMapsCollection implements Serializable{
int opX = nPix % ownd_width + obounds[0][0]; // absolute output pX, pY int opX = nPix % ownd_width + obounds[0][0]; // absolute output pX, pY
int opY = nPix / ownd_width + obounds[1][0]; int opY = nPix / ownd_width + obounds[1][0];
double dX = (opX - scaled_out_center[0]) /scale; // in meters double dX = (opX - scaled_out_center[0]) /scale; // in meters
double dY = (opY - scaled_out_center[1]) /scale; double dY = (opY - scaled_out_center[1]) /scale; // in meters, Y-down
double [] xy_src = { // pixels of the source image double [] xy_src = { // pixels of the source image
src_scale * (affine[0][0]*dX + affine[0][1]*dY + affine[0][2] + src_center[0]), src_scale * (affine[0][0]*dX + affine[0][1]*dY + affine[0][2] + src_center[0]),
src_scale * (affine[1][0]*dX + affine[1][1]*dY + affine[1][2] + src_center[1])}; src_scale * (affine[1][0]*dX + affine[1][1]*dY + affine[1][2] + src_center[1])};
...@@ -7192,6 +7192,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -7192,6 +7192,7 @@ public class OrthoMapsCollection implements Serializable{
indices[i] = i; indices[i] = i;
} }
} }
boolean y_down_ccw = true;
String [] stats = new String[2]; // header, body String [] stats = new String[2]; // header, body
StringBuffer sb = new StringBuffer(); StringBuffer sb = new StringBuffer();
// String head_fmt = "%3s\t%17s\t%26s\t%13s\t%13s\t%7s\t%6s\t%7s\t%6s\t%6s\t%6s\t%6s\n"; // String head_fmt = "%3s\t%17s\t%26s\t%13s\t%13s\t%7s\t%6s\t%7s\t%6s\t%6s\t%6s\t%6s\n";
...@@ -7224,7 +7225,13 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -7224,7 +7225,13 @@ public class OrthoMapsCollection implements Serializable{
double sfm_gain = map.getSfmGain(); double sfm_gain = map.getSfmGain();
int scenes = map.getNumberScenes(); int scenes = map.getNumberScenes();
double [][] affine = map.getAffine(); double [][] affine = map.getAffine();
double [] svd_st = OrthoMap.singularValueDecomposeScaleTilt(affine); SingularValueDecomposition svd_st =SingularValueDecomposition.singularValueDecomposeScaleTiltGamma(
affine,
y_down_ccw); // boolean y_down_ccw)
// double [] svd_st = OrthoMap.singularValueDecomposeScaleTilt(
// affine,
// y_down_ccw); // boolean y_down_ccw)
// svd_st[0] - now gamma - short axis relative to map
sb.append(String.format( sb.append(String.format(
"%3d\t%17s\t%26s\t%11.6f\t%11.6f\t%7.2f\t"+ "%3d\t%17s\t%26s\t%11.6f\t%11.6f\t%7.2f\t"+
"%6.2f\t%7.4f\t%6d\t%6d\t%6d\t%6d\t"+ "%6.2f\t%7.4f\t%6d\t%6d\t%6d\t%6d\t"+
...@@ -7234,7 +7241,7 @@ public class OrthoMapsCollection implements Serializable{ ...@@ -7234,7 +7241,7 @@ public class OrthoMapsCollection implements Serializable{
indx, name, sdt, lla[0], lla[1], lla[2] - agl,// ground level indx, name, sdt, lla[0], lla[1], lla[2] - agl,// ground level
agl, pix_size_cm, width, height, vert_pix[0], vert_pix[1], agl, pix_size_cm, width, height, vert_pix[0], vert_pix[1],
scenes, sfm_gain, scenes, sfm_gain,
svd_st[1],svd_st[2],svd_st[0],svd_st[3], affine[0][2], affine[1][2], svd_st.scale,svd_st.ratio,svd_st.beta,svd_st.rot, affine[0][2], affine[1][2],
affine[0][0], affine[0][1], affine[1][0], affine[1][1])); affine[0][0], affine[0][1], affine[1][0], affine[1][1]));
} }
stats[1] = sb.toString(); stats[1] = sb.toString();
......
src/main/java/com/elphel/imagej/orthomosaic/QuatUtils.java 0 → 100644
View file @ 6a6d0c40
/**
** OrthoMap - Dealing with orthographic maps
**
** Copyright (C) 2024 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** OrthoMap.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.orthomosaic;
public final class QuatUtils { // static class
public static final double [] UNIT_QUAT = {1,0,0,0};
public static final double [][] UNIT_AFFINE = {{1,0},{0,1}};
private QuatUtils() {}
/*
* t= r*s
* (t0, t1, t2, t3) = (r0, r1, r2, r3) * (s0, s1, s2, s3)
* t0 = (r0s0 - r1s1 - r2s2 - r3s3)
* t1 = (r0s1 + r1s0 - r2s3 + r3s2)
* t2 = (r0s2 + r1s3 + r2s0 - r3s1)
* t3 = (r0s3 - r1s2 + r2s1 + r3s0)
*/
/**
* Multiply to quaternions
* @param r first quaternion
* @param s second quaternion
* @return product of two quaternions: r*s
*/
public static double [] multiply(
double [] r,
double [] s
) {
double [] t = {
r[0]*s[0] - r[1]*s[1] - r[2]*s[2] - r[3]*s[3],
r[0]*s[1] + r[1]*s[0] - r[2]*s[3] + r[3]*s[2],
r[0]*s[2] + r[1]*s[3] + r[2]*s[0] - r[3]*s[1],
r[0]*s[3] - r[1]*s[2] + r[2]*s[1] + r[3]*s[0]};
return t;
}
public static double norm(
double [] quat) {
return Math.sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
}
public static double [] invert(
double [] quat) {
return new double [] {quat[0], -quat[1], -quat[2], -quat[3]};
}
public static double [] normalize(
double [] quat) {
// double k = 1/norm(quat);
// return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
return scale (quat, 1/norm(quat));
}
public static double normalizeInPlace(
double [] quat) {
double scale = norm(quat);
for (int i = 0; i < quat.length; i++) {
quat[i] /= scale;
}
return scale;
}
public static double [] scale(
double [] quat,
double k) {
return new double [] {k*quat[0], k*quat[1], k*quat[2], k*quat[3]};
}
/**
* Convert two tilts {tx,ty} Z=x*tx+y*ty to quaternion. Source y is down, quaternion - x - same, y - opposite (positive - up), z - up from the XY plane.
* @param txy {tiltX, tiltY,...} - may contain other values like offset and center
* @param y_down_ccw Invert Y for tilts
* @return plane rotation quaternion rotation righth-hand thread
*/
public static double [] tiltToQuaternion(
double [] txy,
boolean y_down_ccw) { // boolean y_down_ccw)
double tx = txy[0], ty = y_down_ccw?-txy[1]:txy[1]; // invert y
double t2 = tx*tx + ty*ty;
double t = Math.sqrt(t2);
if (t == 0) {
return UNIT_QUAT;
}
double [] axis = {ty/t,-tx/t,0}; // pi/2 CW
/* sin (A/2) = +-sqrt((1 - cos(A))/2)
* cos (A/2) = +-sqrt((1 + cos(A))/2)
* cos(A) = sqrt(t*t+1)
*/
double cos_theta = 1/Math.sqrt(t2+1);
double cos_theta2 = Math.sqrt((1 + cos_theta)/2);
double sin_theta2 = Math.sqrt((1 - cos_theta)/2);
double [] quat = new double[] {cos_theta2, sin_theta2*axis[0], sin_theta2*axis[1], 0};
return quat;
}
/**
* Remove rotation around Z-axis, keep only tilt around axis in XY plane
* @param quat_in input rotation that may include rotation around Z (vertical) axis
* @return nearest rotation around axis in XY plane
*/
public static double [] pureTilt(
double [] quat_in) {
double r0 = quat_in[0], r1 = quat_in[1], r2 = quat_in[2], r3 = quat_in[3];
if (r0 == 0) {
return UNIT_QUAT;
}
double r3_r0 = r3/r0;
// find {s0,0,0,s3} - rotation around vertical axis so r*s will not have Z component
double s0 = Math.sqrt(1/(1 + r3_r0*r3_r0));
double s3 = -r3_r0*s0;
double [] quat = {
r0 * s0 - r3 * s3,
r1 * s0 - r2 * s3,
r1 * s3 + r2 * s0,
r0 * s3 + r3 * s0
};
return quat;
}
/**
* Normalize quaterion (in-place) and return its original length to be used as scale
* @param quat quaternion with encoded scale, will be normalized in place
* @return length of the input quaternion before normalization;
*/
public static double normQuat(double [] quat) {
double s2 = 0;
for (double q:quat) {
s2+= q*q;
}
double l = Math.sqrt(s2);
for (int i = 0; i < quat.length;i++) {
quat[i] /= l;
}
return l;
}
/**
* Convert rotation around some axis in XY plane to affine transformation
* @param quat rotation, quat[3]~=0 (y-up)
* @param stretch project to tilted plane (stretch perpendicular to rotation axis)
* @param make__pure_tilt rotate around vertical axis (Z) to make quat[3]=0
* @return [2][2] affine transform ( y-down)
*/
public static double [][] quatToAffine(
double [] quat,
boolean stretch,
boolean make__pure_tilt,
boolean y_down_ccw){
// TODO: use scale
double scale = normQuat(quat);
double y_sign = y_down_ccw?-1:1; // invert y from quaternions to affines
if (make__pure_tilt) {
quat = pureTilt(quat);
}
double ax = quat[1], ay = quat[2] * y_sign;
double l = Math.sqrt(ax*ax+ay*ay);
if (l == 0) {
return UNIT_AFFINE;
}
ax /= l;
ay /= l;
double cos_theta = 2 * quat[0]*quat[0] - 1; // cos(2 * A) = 2 * cos(A)^2 -1
double k = stretch ? (1/cos_theta) : cos_theta;
double s = k - 1;
double ax2 = ax*ax, ay2=ay*ay, axy = ax*ay;
double [][] affine = {
// {1 + s * ax * ax, s * ax * ay},
// { - s * ax * ay , 1 + s * ay * ay}};
/// {k*ax2 + ay2,-s*axy},
/// {-s*axy , ax2 + k*ay2}};
{1 + s*ay2, -s*axy},
{ -s*axy ,1 + s*ax2}};
// {1 + s*ay2, s*axy},
// { s*axy ,1 + s*ax2}};
for (int i = 0; i < 2; i++) {
for (int j=0; j < 2; j++) {
affine[i][j] *= scale;
}
}
return affine;
}
/**
* Restore quaternion (rotation+scale) from affine traqnsform (only [2][2] is used, ok to have [2][3] input
* As there are 2 solutions, both are provided in the output. As the
* input linear transformation matrix converts ground coordinates to source
* image coordinates, the scale in the tilt direction is > than scale in the
* perpendicular direction (tilt axis).
* @param affine affine transform, only [2][2] top-left subarray is used
* @param y_down_ccw if true, affine corresponds to to y-down (image) coordinate system
* @return a pair of quaternions including scale (sqrt(q0^2+q1^2+q2^2+q3^2)
*/
public static double [][] affineToQuatScaled (
double [][] affine,
boolean y_down_ccw) {
SingularValueDecomposition svd= SingularValueDecomposition.singularValueDecomposeScaleTiltBeta(
affine,
y_down_ccw);
double scale = svd.getMinScale();
double rot = svd.getRotAngle(); // TODO: check sign !
// Now beta,rot correspond to Y - up
double chalfrot = Math.cos(rot/2);
double shalfrot = Math.sin(rot/2);
double [] qrot = {chalfrot,0,0,shalfrot}; // rotation around vertical axis
double cbeta = Math.cos(svd.beta);
double sbeta = Math.sin(svd.beta); // TODO: check sign !
double tiltAngle = svd.getTiltAngle(); // >0
double ctilt = Math.cos(tiltAngle/2);
double stilt = Math.sin(tiltAngle/2);
double [] q_plus = {ctilt, stilt*cbeta, stilt*sbeta, 0};
double [] q_minus = {ctilt, -stilt*cbeta, -stilt*sbeta, 0};
double [][] quats_pm = {scale(multiply(q_plus, qrot), scale),scale(multiply(q_minus, qrot), scale)};
return quats_pm;
}
public static double [][] matMult (
double [][] m1,
double [][] m2){
if (m1[0].length != m2.length) {
System.out.println("matMul(): m1[0].length != m2.length ("+m1[0].length+" !="+m2.length+")");
return null;
}
double [][] m = new double[m1.length][m2[0].length];
for (int i = 0; i < m.length; i++) {
for (int j = 0; j < m[i].length; j++) {
for (int k = 0; k < m1.length; k++) {
m[i][j] += m1[i][k]*m2[k][j];
}
}
}
return m;
}
public static double [] matMul (
double [][] m1,
double [] v){
if (m1[0].length != v.length) {
System.out.println("matMul(): m1[0].length != v.length ("+m1[0].length+" !="+v.length+")");
return null;
}
double [] v2 = new double[v.length];
for (int i = 0; i < m1.length; i++) {
for (int k = 0; k < m1.length; k++) {
v2[i] += m1[i][k]*v[k];
}
}
return v2;
}
}
src/main/java/com/elphel/imagej/orthomosaic/SingularValueDecomposition.java 0 → 100644
View file @ 6a6d0c40
package com.elphel.imagej.orthomosaic;
public class SingularValueDecomposition {
double beta;
double gamma;
double w1;
double w2;
double rot;
double scale; // min (w1,w2) or sqrt(w1*w2) for raw singularValueDecompose()
double ratio; // <=1.0, ==cos(tilt) or w2/w1 for raw singularValueDecompose()
boolean st= false;
boolean y_down_ccw;
public SingularValueDecomposition(
double beta,
double gamma,
double w1,
double w2,
double rot) {
this.beta = beta;
this.gamma = gamma;
this.w1 = w1;
this.w2 = w2;
this.rot = rot;
}
public static double [][] rotMatrix(double a){
double c = Math.cos(a);
double s = Math.sin(a);
return new double [][] {{c,s},{-s,c}};
}
// A = getR1() * getW() getR2() = getR1() * getW() * getIR1() * getRot()
public double [][] getR1(){ return rotMatrix(beta);}
public double [][] getR2(){ return rotMatrix(gamma);}
public double [][] getIR1(){return rotMatrix(-beta);}
public double [][] getRot(){return rotMatrix(rot);}
public double getTiltAngle() {
if (ratio <= 1.0) return Math.acos(ratio);
else return Math.acos(1/ratio);
}
public double getRotAngle() {return rot;}
public double getScale() {return scale;}
public double getAvgScale() {return Math.sqrt(w1*w2);}
public double getMinScale() {return Math.min(w1,w2);}
public double getMaxScale() {return Math.max(w1,w2);}
public double [][] getW(){
return new double [][] {{w1,0},{0,w1}};
}
/**
* Decomposing linear transform into rotation
* R1={{cos(beta),sin(beta)},{-sin(beta), cos(beta)}} ,
* non-uniform scale transform W={{w1,0}{0,w2}}, and rotation
* R2={{cos(gamma),sin(gamma)},{-sin(gamma), cos(gamma)}}
* A=R1*W*R2 using singular value decomposition -- tested
* https://math.stackexchange.com/questions/861674/decompose-a-2d-arbitrary-transform-into-only-scaling-and-rotation
* @param A - input 2x2 matrix
* @return {s,beta,w,gamma,beta+gamma}
* A = B + C
* b00= b11; c00=-c01
* b10=-b01; c10= c01
*/
public static SingularValueDecomposition singularValueDecompose(
double [][] A) {
double a00=A[0][0],a01=A[0][1],a10=A[1][0],a11=A[1][1];
double b00=(a00+a11)/2; // , b11 = b00;
double c00=(a00-a11)/2; //, c11 =-c00;
double b01=(a01-a10)/2; //, b10 =-b01;
double c01=(a01+a10)/2; //, c10 = c01;
double w1_p_w2_2= Math.sqrt(b00*b00+b01*b01);
double w1_m_w2_2= Math.sqrt(c00*c00+c01*c01);
double w1 = w1_p_w2_2 + w1_m_w2_2;
double w2 = w1_p_w2_2 - w1_m_w2_2;
double g_p_b = Math.atan2(b01, b00);
double g_m_b = Math.atan2(c01, c00);
double gamma = (g_p_b + g_m_b)/2;
double beta = (g_p_b - g_m_b)/2;
SingularValueDecomposition svd= new SingularValueDecomposition (beta,gamma,w1,w2,g_p_b);
svd.scale = Math.sqrt(svd.w1*svd.w2);
svd.ratio = svd.w2/svd.w1;
return svd;
}
/**
* Use singular value decomposition and then split scaling {{w1,0},{0,w1}}
* into overall scaling caused by zoom != 1.0 because of altitude error
* and unidirectional scaling caused by tilted projection plane. As the
* input linear transformation matrix converts ground coordinates to source
* image coordinates, the scale in the tilt direction is > than scale in the
* perpendicular direction (tilt axis).
* Matrix R1 is additionally rotated by PI/2 if needed so W={{w1,0},{0,w2}}
* has w2>=w1 and W={{s,0},{0,s/t}}, where t <= 1.0 and equals to cos(tilt)
*
* @param A - linear transformation matrix from rectified ground coordinates
* to source image coordinates. OK to use 2x3 affine matrix,extra
* components will be ignored.
* @param y_down_ccw - positive Y is down, positive angles are CCW
* @return SingularValueDecomposition instance with scale and ratio (<=1) defined, gamma and beta updated
*/
public static SingularValueDecomposition singularValueDecomposeScaleTiltGamma(
double [][] A,
boolean y_down_ccw) {
SingularValueDecomposition svd = singularValueDecompose(A);
svd.setScaleTiltGamma(y_down_ccw);
return svd;
}
public static SingularValueDecomposition singularValueDecomposeScaleTiltBeta(
double [][] A,
boolean y_down_ccw) {
SingularValueDecomposition svd = singularValueDecompose(A);
svd.setScaleTiltBeta(y_down_ccw);
return svd;
}
public void setScaleTiltGamma(
boolean y_down_ccw) {
// For Y-down, angles are CW positive, for Y-up angles are CCW positive
// Affines are Y-up?
// considering tilt in y direction (unless long_axis), it should have higher scale
// (and source image coordinates), while X should correspond to
// the axis of rotation, and scale is just scale caused by error in
// altitude.
scale = Math.min(w1, w2);
ratio = w1/w2; // <=1.0, ==cos(tilt)
if (w1 > w2) { // rotate tilt by PI/2
ratio = w2/w1;
gamma += Math.PI/2; // start with rotation (last in matrices)
if (gamma > Math.PI) {
gamma -= 2* Math.PI;
}
}
if (gamma > Math.PI/2) {
gamma -= Math.PI;
} else if (gamma < -Math.PI) {
gamma += Math.PI;
}
beta = rot-gamma;
while (beta >= Math.PI/2) {
beta -= Math.PI;
}
while (beta < Math.PI/2) {
beta += Math.PI;
}
if (y_down_ccw) {
gamma *= -1;
beta *= -1;
rot *= -1; // pure rotation
}
this.y_down_ccw = y_down_ccw;
st = true;
}
public void setScaleTiltBeta(
boolean y_down_ccw) {
// For Y-down, angles are CW positive, for Y-up angles are CCW positive
// Affines are Y-up?
// considering tilt in y direction (unless long_axis), it should have higher scale
// (and source image coordinates), while X should correspond to
// the axis of rotation, and scale is just scale caused by error in
// altitude.
scale = Math.min(w1, w2);
ratio = w1/w2; // <=1.0, ==cos(tilt)
if (w1 > w2) { // rotate tilt by PI/2
ratio = w2/w1;
beta += Math.PI/2; // start with rotation (last in matrices)
if (beta > Math.PI) {
beta -= 2* Math.PI;
}
}
if (beta >= Math.PI/2) {
beta -= Math.PI;
} else if (beta < -Math.PI) {
beta += Math.PI;
}
gamma = rot-beta;
while (gamma >= Math.PI/2) {
gamma -= Math.PI;
}
while (gamma < Math.PI/2) {
gamma += Math.PI;
}
if (y_down_ccw) {
beta *= -1;
gamma *= -1;
rot *= -1; // pure rotation
}
this.y_down_ccw = y_down_ccw;
st = true;
}
}
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