Commit 86cdc8a0 authored by Andrey Filippov's avatar Andrey Filippov

debugging planes merging

parent 1361f670
......@@ -2149,6 +2149,7 @@ public class EyesisCorrectionParameters {
public int plMaxOutliers = 20; // Maximal number of outliers to remove
public double plMinStrength = 0.1; // Minimal total strength of a plane
public double plMaxEigen = 0.3; // Maximal eigenvalue of a plane
public boolean plDbgMerge = true; // Combine 'other' plane with current
// other debug images
public boolean show_ortho_combine = false; // Show 'ortho_combine'
......@@ -2389,6 +2390,7 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"plMaxOutliers", this.plMaxOutliers+"");
properties.setProperty(prefix+"plMinStrength", this.plMinStrength +"");
properties.setProperty(prefix+"plMaxEigen", this.plMaxEigen +"");
properties.setProperty(prefix+"plDbgMerge", this.plDbgMerge+"");
properties.setProperty(prefix+"show_ortho_combine", this.show_ortho_combine+"");
properties.setProperty(prefix+"show_refine_supertiles", this.show_refine_supertiles+"");
......@@ -2620,6 +2622,7 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"plMaxOutliers")!=null) this.plMaxOutliers=Integer.parseInt(properties.getProperty(prefix+"plMaxOutliers"));
if (properties.getProperty(prefix+"plMinStrength")!=null) this.plMinStrength=Double.parseDouble(properties.getProperty(prefix+"plMinStrength"));
if (properties.getProperty(prefix+"plMaxEigen")!=null) this.plMaxEigen=Double.parseDouble(properties.getProperty(prefix+"plMaxEigen"));
if (properties.getProperty(prefix+"plDbgMerge")!=null) this.plDbgMerge=Boolean.parseBoolean(properties.getProperty(prefix+"plDbgMerge"));
if (properties.getProperty(prefix+"show_ortho_combine")!=null) this.show_ortho_combine=Boolean.parseBoolean(properties.getProperty(prefix+"show_ortho_combine"));
if (properties.getProperty(prefix+"show_refine_supertiles")!=null) this.show_refine_supertiles=Boolean.parseBoolean(properties.getProperty(prefix+"show_refine_supertiles"));
......@@ -2873,6 +2876,7 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Maximal number of outliers to remove", this.plMaxOutliers, 0);
gd.addNumericField("Minimal total strength of a plane", this.plMinStrength, 6);
gd.addNumericField("Maximal eigenvalue of a plane", this.plMaxEigen, 6);
gd.addCheckbox ("Combine 'other' plane with the current", this.plDbgMerge);
gd.addMessage ("--- Other debug images ---");
gd.addCheckbox ("Show 'ortho_combine'", this.show_ortho_combine);
......@@ -3113,6 +3117,7 @@ public class EyesisCorrectionParameters {
this.plMaxOutliers= (int) gd.getNextNumber();
this.plMinStrength= gd.getNextNumber();
this.plMaxEigen= gd.getNextNumber();
this.plDbgMerge= gd.getNextBoolean();
this.show_ortho_combine= gd.getNextBoolean();
this.show_refine_supertiles= gd.getNextBoolean();
......
......@@ -1067,7 +1067,7 @@ public class SuperTiles{
// final int debug_stile = 18 * stilesX + 25;
// final int debug_stile = 20 * stilesX + 24;
final int debug_stile = 17 * stilesX + 10;
final int debug_stile = 16 * stilesX + 27; // 10;
for (int ithread = 0; ithread < threads.length; ithread++) {
......@@ -1121,10 +1121,11 @@ public class SuperTiles{
// int dl = ((nsTile >= debug_stile-1) && (nsTile <= debug_stile+1) ) ? 1 : 0;
// int dl = ((stileY == 17) && (stileX > 4)) ? 1 : 0;
// int dl = (stileY >= 0) ? 1 : 0;
int dl1 = (nsTile == debug_stile-1) ? 3 : 0;
int dl1 = (nsTile == debug_stile) ? 3 : 0;
// int dl = (nsTile == debug_stile) ? 3 : 0;
int dl = ((stileY >= 15) && (stileY <= 18) && (stileX >= 5) && (stileX <= 31)) ? 1 : 0;
// int dl = ((stileY == 17) && (stileX == 27) ) ? 3 : 0;
// int dl = ((stileY >= 15) && (stileY <= 18) && (stileX >= 5) && (stileX <= 31)) ? 1 : 0;
int dl = ((stileY == 16) && (stileX == 27) ) ? 3 : 0;
// int debugLevel1 = ((sTileXY[0] == 27) && (sTileXY[1] == 16))? 1: 0; // check why v[0][0] <0
boolean [] sel_all = stSel.clone();
......@@ -1301,6 +1302,9 @@ public class SuperTiles{
}
if (st_planes.size() > 0){
planes[nsTile] = st_planes.toArray(new TilePlanes.PlaneData[0] );
if (dl >0){
System.out.println("processPlanes2(): nsTile="+nsTile);
}
}
} // if sw >0
......@@ -1325,13 +1329,9 @@ public class SuperTiles{
return wh;
}
// public double [][]
TilePlanes.PlaneData [][] getNeibPlanes(
final int dir // 0: get from up (N), 1:from NE, ... 7 - from NW
// final double minWeight,
// final double maxEigen,
// final double dispNorm,
// final boolean use_NaN
final int dir, // 0: get from up (N), 1:from NE, ... 7 - from NW
final boolean dbgMerge // Combine 'other' plane with current
)
{
final int tilesX = tileProcessor.getTilesX();
......@@ -1340,16 +1340,13 @@ public class SuperTiles{
final int tileSize = tileProcessor.getTileSize();
final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize;
// final int width = stilesX * superTileSize; // * tileSize;
// final int height = stilesY * superTileSize; // * tileSize;
final double [] nan_plane = new double [superTileSize*superTileSize];
for (int i = 0; i < nan_plane.length; i++) nan_plane[i] = Double.NaN;
// final int centerIndex = (superTileSize+1) * superTileSize / 2;
final int [][] dirsYX = {{-1, 0},{-1,1},{0,1},{1,1},{1,0},{1,-1},{0,-1},{-1,-1}};
TilePlanes.PlaneData [][] neib_planes = new TilePlanes.PlaneData[stilesY * stilesX][];
TilePlanes tpl = null; // new TilePlanes(tileSize,superTileSize, geometryCorrection);
// final int debug_stile = 17 * stilesX + 27;
final int debug_stile = 20 * stilesX + 27;
// final int debug_stile = 20 * stilesX + 27;
final int debug_stile = 17 * stilesX + 27;
for (int sty0 = 0; sty0 < stilesY; sty0++){
int sty = sty0 + dirsYX[dir][0];
......@@ -1377,9 +1374,55 @@ public class SuperTiles{
if (nsTile0 == debug_stile) {
System.out.println("getNeibPlanes(): nsTile0="+nsTile0+", nsTile="+nsTile+", np = "+np+" dir = "+ dir );
}
neib_planes[nsTile0][np] = pd.getPlaneToThis(
TilePlanes.PlaneData other_pd = pd.getPlaneToThis(
planes[nsTile][np], // PlaneData otherPd,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
int num_this = (planes[nsTile0] == null)? 0:(planes[nsTile0].length);
int best_index = 0;
if (dbgMerge && (num_this > 0)) {
double best_value = Double.NaN;
if (num_this > 1){
// find the best candidate for merge
for (int indx = 1; indx < planes[nsTile0].length; indx++){
if (nsTile0 == debug_stile) {
System.out.println("this with this, same weight:");
TilePlanes.PlaneData this_this_pd = planes[nsTile0][indx].mergePlaneToThis(
planes[nsTile0][indx], // PlaneData otherPd,
1.0, // double scale_other,
true, // boolean ignore_weights,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
System.out.println("other with other, same weight:");
TilePlanes.PlaneData other_other_pd = other_pd.mergePlaneToThis(
other_pd, // PlaneData otherPd,
1.0, // double scale_other,
true, // boolean ignore_weights,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
System.out.println("other with this, same weight:");
}
TilePlanes.PlaneData merged_pd = planes[nsTile0][indx].mergePlaneToThis(
other_pd, // PlaneData otherPd,
1.0, // double scale_other,
true, // boolean ignore_weights,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
if (!(merged_pd.getValue() > best_value)) { // Double.isNaN(best_value) will work too
best_value = merged_pd.getValue();
best_index = indx;
}
}
}
// now merge with weights with the best plane of this supertile
if (nsTile0 == debug_stile) {
System.out.println("this with this, proportional weight, np(other):"+np+" best fit(this):"+best_index);
}
TilePlanes.PlaneData merged_pd = planes[nsTile0][best_index].mergePlaneToThis(
other_pd, // PlaneData otherPd,
1.0, // double scale_other,
false, // boolean ignore_weights,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
neib_planes[nsTile0][np] = merged_pd;
} else {
neib_planes[nsTile0][np] = other_pd;
}
}
} else {
neib_planes[nsTile0] = null;
......
......@@ -56,7 +56,8 @@ public class TilePlanes {
double [] world_xyz = null; // world coordinates of the nearest point of the plane, in meters
double [] world_v1 = null; // world in-plane vector, corresponding to vectors[1]
double [] world_v2 = null; // world in-plane vector, corresponding to vectors[1]
double [] daxy = null; // disparity and 2 relative angles (ax and ay) corresponding to fisheye view, near (0,0) scale is pixel size
// double [] daxy = null; // disparity and 2 relative angles (ax and ay) corresponding to fisheye view, near (0,0) scale is pixel size
double [][] neib_match = null; // for each of the directions (N, NE, .. NW) quality match for each layer
int tileSize;
int superTileSize;
int [] sTileXY = null; // X and Y indices of this superTile in the image
......@@ -76,7 +77,7 @@ public class TilePlanes {
if (this.world_xyz != null) pd.world_xyz = this.world_xyz.clone();
if (this.world_v1 != null) pd.world_v1 = this.world_v1.clone();
if (this.world_v2 != null) pd.world_v2 = this.world_v2.clone();
if (this.daxy != null) pd.daxy = this.daxy.clone();
// if (this.daxy != null) pd.daxy = this.daxy.clone();
if (this.vectors != null) {
pd.vectors = new double[3][];
pd.vectors[0] = this.vectors[0].clone();
......@@ -233,6 +234,153 @@ public class TilePlanes {
return new Matrix (ar);
}
/**
* Combine 2 Plane instances using centers, eigenvalues eihenvectors and total weights of this and other PlaneData objects
* other plane should already be transformed to the same supertile coordinate system (with getPlaneToThis() method)
* @param otherPd PlaneData object to merge with. Should be transformed to the same supertile ( same sTileXY values)
* @param scale_other scale total weight of the other PlaneData object
* @param ignore_weights assume original weights of the planes where equal, only use scale_other
* @param debugLevel debug level
* @return PlaneData object representing merged planes with combined weight (scale_other*otherPd.weight + this.weight),
* recalculated center, eigenvalues and eigenvectors
*/
public PlaneData mergePlaneToThis(
PlaneData otherPd,
double scale_other,
boolean ignore_weights,
int debugLevel)
{
if (debugLevel > 0) {
System.out.println("mergePlaneToThis()");
}
double [][] this_eig_avals = {
{values[0], 0.0, 0.0},
{0.0, values[1], 0.0},
{0.0, 0.0, values[2]}};
double [][] other_eig_avals = {
{otherPd.values[0], 0.0, 0.0},
{0.0, otherPd.values[1], 0.0},
{0.0, 0.0, otherPd.values[2]}};
Matrix this_eig_vals = new Matrix(this_eig_avals);
Matrix other_eig_vals = new Matrix(other_eig_avals);
Matrix other_eig_vectors = new Matrix(otherPd.vectors).transpose(); // vectors are saved as rows
Matrix this_eig_vectors = new Matrix(this.vectors).transpose(); // vectors are saved as rows
Matrix this_center = new Matrix(this.getZxy(),3);
Matrix other_center = new Matrix(otherPd.getZxy(),3); // should already be relative to this supertile center
double sum_weight = scale_other * otherPd.weight + this.weight;
double other_fraction = ignore_weights? (scale_other/(scale_other + 1.0)): ((scale_other * otherPd.weight) / sum_weight);
Matrix common_center = this_center.times(1.0 - other_fraction).plus(other_center.times(other_fraction));
Matrix other_offset = other_center.minus(this_center); // other center from this center
if (debugLevel > 0) {
System.out.println("other_eig_vals");
other_eig_vals.print(8, 6);
System.out.println("this_eig_vals");
this_eig_vals.print(8, 6);
System.out.println("other_eig_vectors");
other_eig_vectors.print(8, 6);
System.out.println("this_eig_vectors");
this_eig_vectors.print(8, 6);
System.out.println("other_center");
other_center.print(8, 6);
System.out.println("this_center");
this_center.print(8, 6);
System.out.println("common_center");
common_center.print(8, 6);
System.out.println("other_offset");
other_offset.print(8, 6);
System.out.println("other_fraction="+other_fraction);
}
double [][] acovar = { // covariance matrix of center masses (not yet scaled by weight)
{other_offset.get(0,0)*other_offset.get(0,0), other_offset.get(0,0)*other_offset.get(1,0), other_offset.get(0,0)*other_offset.get(2,0)},
{other_offset.get(1,0)*other_offset.get(0,0), other_offset.get(1,0)*other_offset.get(1,0), other_offset.get(1,0)*other_offset.get(2,0)},
{other_offset.get(2,0)*other_offset.get(0,0), other_offset.get(2,0)*other_offset.get(1,0), other_offset.get(2,0)*other_offset.get(2,0)}};
Matrix other_covar = other_eig_vectors.times(other_eig_vals.times(other_eig_vectors.transpose()));
Matrix this_covar = this_eig_vectors.times(this_eig_vals.times(this_eig_vectors.transpose()));
Matrix covar = (new Matrix(acovar)).times(other_fraction*(1.0-other_fraction)); // only centers with all masses
if (debugLevel > 0) {
System.out.println("other_covar");
other_covar.print(8, 6);
System.out.println("this_covar");
this_covar.print(8, 6);
System.out.println("covar");
covar.print(8, 6);
}
covar.plusEquals(other_covar.times(other_fraction));
if (debugLevel > 0) {
System.out.println("covar with other_covar");
covar.print(8, 6);
}
covar.plusEquals(this_covar.times(1.0 - other_fraction));
if (debugLevel > 0) {
System.out.println("covar with other_covar and this_covar");
covar.print(8, 6);
}
// extract new eigenvalues, eigenvectors
EigenvalueDecomposition eig = covar.eig();
// eig.getD().getArray(),
// eig.getV().getArray(),
if (debugLevel > 0) {
System.out.println("eig.getV()");
eig.getV().print(8, 6);
System.out.println("eig.getD()");
eig.getD().print(8, 6);
}
double [][] eig_vect = eig.getV().getArray();
double [][] eig_val = eig.getD().getArray();
// make towards camera, left coordinate system
int oindx = 0;
for (int i = 1; i <3; i++){
if (Math.abs(eig_vect[0][i]) > Math.abs(eig_vect[0][oindx])){
oindx = i;
}
}
// select 2 other axes for increasing eigenvalues (so v is short axis, h is the long one)
int vindx = (oindx == 0)? 1 : 0;
int hindx = (oindx == 0)? 2 : ((oindx == 1) ? 2 : 1);
if (eig_val[vindx][vindx] > eig_val[hindx][hindx]){
int tmp = vindx;
vindx = hindx;
hindx = tmp;
}
double [][] plane = {
{eig_vect[0][oindx],eig_vect[1][oindx],eig_vect[2][oindx]}, // plane normal to camera
{eig_vect[0][vindx],eig_vect[1][vindx],eig_vect[2][vindx]}, // "horizontal" axis // to detect skinny planes and poles
{eig_vect[0][hindx],eig_vect[1][hindx],eig_vect[2][hindx]}}; // "vertical" axis // to detect skinny planes and poles
// Make normal be towards camera (positive disparity), next vector - positive in X direction (right)
for (int v = 0; v < 2; v++) {
if (plane[v][v] < 0.0) for (int i = 0; i < 3; i ++) plane[v][i] = -plane[v][i];
}
// make direction last vector so px (x) py (.) disp < 0 (left-hand coordinate system)
if (new Matrix(plane).det() > 0){
for (int i = 0; i < 3; i ++) plane[2][i] = -plane[2][i];
}
PlaneData pd = this.clone(); // will copy selections too
pd.setValues(eig_val[oindx][oindx],eig_val[vindx][vindx],eig_val[hindx][hindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
pd.setVectors(plane);
pd.setZxy(common_center.getColumnPackedCopy()); // set new center
// what weight to use? cloned is original weight for this supertile
// or use weighted average like below?
pd.setWeight(other_fraction * otherPd.weight + (1.0 - other_fraction) * this.weight);
return pd;
}
/**
* Convert plane data from other supertile to this one (disparity, px, py) for the center of this supertile
* through the plane in world coordinates. orientation of the "main" eigenvector (most disparity-like) is
......@@ -299,7 +447,10 @@ public class TilePlanes {
// now get both in-plane vectors transformed to this supertile disparity, px, py
Matrix v1 = img_jacobian.times(new Matrix(wv1,3)); // 3 rows, 1 column
Matrix v2 = img_jacobian.times(new Matrix(wv2,3)); // 3 rows, 1 column
Matrix v0 = cross3d(v1,v2); // orthogonal to both - this is a plane normal vector in local supertile disparity, px. py
// Matrix v0 = cross3d(v1,v2); // orthogonal to both - this is a plane normal vector in local supertile disparity, px. py
Matrix v0 = cross3d(v2,v1); // orthogonal to both - this is a plane normal vector in local supertile disparity, px. py
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), Matrix v0 =");
......@@ -356,6 +507,13 @@ public class TilePlanes {
System.out.println("getPlaneToThis(), dxy(modified) = {"+ dxy[0]+", "+ dxy[1]+", "+ dxy[2]+"}");
}
pd.sTileXY = this.sTileXY;
if (pd.vectors[0][0] > 0) {
// System.out.println("getPlaneToThis(): "+pd.sTileXY[0]+":"+pd.sTileXY[1]+" -> "+pd.vectors[0][0]+", disp = "+disp+
// ", other_det = "+((new Matrix(otherPd.vectors).det()) +", pdr_det = "+((new Matrix(pd.vectors).det()))));
} else {
// System.out.println("getPlaneToThis(): "+pd.sTileXY[0]+":"+pd.sTileXY[1]+" -> "+pd.vectors[0][0]+", disp = "+disp+
// ", other_det = "+((new Matrix(otherPd.vectors).det()) +", pdr_det = "+((new Matrix(pd.vectors).det()))));
}
return pd; // make sure pd are updated // "this" is not used. Should it be used instead of pd?
}
......@@ -598,12 +756,17 @@ public class TilePlanes {
select = new boolean [4*stSize];
for (int i = 0; i < select.length; i++) select[i] = true;
}
// int debugLevel1 = ((sTileXY[0] == 27) && (sTileXY[1] == 20))? 1: 0;
// int debugLevel1 = ((sTileXY[0] == 27) && (sTileXY[1] == 17))? 1: 0;
int debugLevel1 = ((sTileXY[0] == 27) && (sTileXY[1] == 16))? 1: 0; // check why v[0][0] <0
double [][][] rslt = getCovar(
data,
weight,
select,
0.0,
0); // debugLevel);
debugLevel1); //0); // debugLevel);
if (rslt == null) return null;
int numPoints = (int) rslt[2][0][2];
// double kz = rslt[2][0][1]; // == 1.0
......@@ -645,7 +808,8 @@ public class TilePlanes {
this.geometryCorrection);
pd.setZxy(szxy);
pd.setValues(eig_val[oindx][oindx],eig_val[hindx][hindx],eig_val[vindx][vindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
// pd.setValues(eig_val[oindx][oindx],eig_val[hindx][hindx],eig_val[vindx][vindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
pd.setValues(eig_val[oindx][oindx],eig_val[vindx][vindx],eig_val[hindx][hindx]); // eigenvalues [0] - thickness, 2 other to detect skinny (poles)
double [][] plane = {
{eig_vect[0][oindx],eig_vect[1][oindx],eig_vect[2][oindx]}, // plane normal to camera
......
......@@ -3000,7 +3000,9 @@ public class TileProcessor {
sdfa_instance.showArrays(plane_data, wh[0], wh[1], true, "plane_data");
// show plane data
for (int dr = 0; dr < 8; dr++){
TilePlanes.PlaneData [][] planes = st.getNeibPlanes(dr);
TilePlanes.PlaneData [][] planes = st.getNeibPlanes(
dr,
clt_parameters.plDbgMerge);
plane_data_nonan = st.getShowPlanes(
planes,
clt_parameters.plMinStrength, // minWeight,
......
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