Skip to content

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

planes from neighbor supertiles

parent 3f6c6c63
Hide whitespace changes
Inline Side-by-side
Showing with 601 additions and 64 deletions
src/main/java/EyesisCorrectionParameters.java
View file @ 1361f670
......@@ -2147,7 +2147,8 @@ public class EyesisCorrectionParameters {
public double plTargetEigen = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
public double plFractOutliers = 0.3; // Maximal fraction of outliers to remove
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
// other debug images
public boolean show_ortho_combine = false; // Show 'ortho_combine'
......@@ -2163,7 +2164,7 @@ public class EyesisCorrectionParameters {
public boolean show_neighbors = false; // show 'neighbors'
public boolean show_flaps_dirs = false; // show 'flaps-dirs'
public boolean show_first_clusters = false; // show 'first_N_clusters'
public boolean show_planes = false; // show planes
public CLTParameters(){}
public void setProperties(String prefix,Properties properties){
......@@ -2386,6 +2387,8 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"plTargetEigen", this.plTargetEigen +"");
properties.setProperty(prefix+"plFractOutliers", this.plFractOutliers +"");
properties.setProperty(prefix+"plMaxOutliers", this.plMaxOutliers+"");
properties.setProperty(prefix+"plMinStrength", this.plMinStrength +"");
properties.setProperty(prefix+"plMaxEigen", this.plMaxEigen +"");
properties.setProperty(prefix+"show_ortho_combine", this.show_ortho_combine+"");
properties.setProperty(prefix+"show_refine_supertiles", this.show_refine_supertiles+"");
......@@ -2399,6 +2402,7 @@ public class EyesisCorrectionParameters {
properties.setProperty(prefix+"show_neighbors", this.show_neighbors+"");
properties.setProperty(prefix+"show_flaps_dirs", this.show_flaps_dirs+"");
properties.setProperty(prefix+"show_first_clusters", this.show_first_clusters+"");
properties.setProperty(prefix+"show_planes", this.show_planes+"");
}
public void getProperties(String prefix,Properties properties){
if (properties.getProperty(prefix+"transform_size")!=null) this.transform_size=Integer.parseInt(properties.getProperty(prefix+"transform_size"));
......@@ -2614,6 +2618,8 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"plTargetEigen")!=null) this.plTargetEigen=Double.parseDouble(properties.getProperty(prefix+"plTargetEigen"));
if (properties.getProperty(prefix+"plFractOutliers")!=null) this.plFractOutliers=Double.parseDouble(properties.getProperty(prefix+"plFractOutliers"));
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+"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"));
......@@ -2627,6 +2633,7 @@ public class EyesisCorrectionParameters {
if (properties.getProperty(prefix+"show_neighbors")!=null) this.show_neighbors=Boolean.parseBoolean(properties.getProperty(prefix+"show_neighbors"));
if (properties.getProperty(prefix+"show_flaps_dirs")!=null) this.show_flaps_dirs=Boolean.parseBoolean(properties.getProperty(prefix+"show_flaps_dirs"));
if (properties.getProperty(prefix+"show_first_clusters")!=null) this.show_first_clusters=Boolean.parseBoolean(properties.getProperty(prefix+"show_first_clusters"));
if (properties.getProperty(prefix+"show_planes")!=null) this.show_planes=Boolean.parseBoolean(properties.getProperty(prefix+"show_planes"));
}
public boolean showDialog() {
......@@ -2864,6 +2871,8 @@ public class EyesisCorrectionParameters {
gd.addNumericField("Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below", this.plTargetEigen, 6);
gd.addNumericField("Maximal fraction of outliers to remove", this.plFractOutliers, 6);
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.addMessage ("--- Other debug images ---");
gd.addCheckbox ("Show 'ortho_combine'", this.show_ortho_combine);
......@@ -2878,6 +2887,7 @@ public class EyesisCorrectionParameters {
gd.addCheckbox ("show 'neighbors'", this.show_neighbors);
gd.addCheckbox ("Show 'flaps-dirs'", this.show_flaps_dirs);
gd.addCheckbox ("Show 'first_N_clusters'", this.show_first_clusters);
gd.addCheckbox ("Show planes", this.show_planes);
WindowTools.addScrollBars(gd);
gd.showDialog();
......@@ -3101,6 +3111,8 @@ public class EyesisCorrectionParameters {
this.plTargetEigen= gd.getNextNumber();
this.plFractOutliers= gd.getNextNumber();
this.plMaxOutliers= (int) gd.getNextNumber();
this.plMinStrength= gd.getNextNumber();
this.plMaxEigen= gd.getNextNumber();
this.show_ortho_combine= gd.getNextBoolean();
this.show_refine_supertiles= gd.getNextBoolean();
......@@ -3114,6 +3126,7 @@ public class EyesisCorrectionParameters {
this.show_neighbors= gd.getNextBoolean();
this.show_flaps_dirs= gd.getNextBoolean();
this.show_first_clusters= gd.getNextBoolean();
this.show_planes= gd.getNextBoolean();
return true;
}
}
......
src/main/java/GeometryCorrection.java
View file @ 1361f670
......@@ -356,6 +356,36 @@ public class GeometryCorrection {
return xyz;
}
/**
* Find disparity for the intersection of the view ray (px, py) and a real-world plane orthogonal through the end of the
* vector norm_xyz
* @param norm_xyz vector from the origin (camera) orthogonal to the plane, length is a distance to the plane
* @param px pixel coordinate horizontal
* @param py pixel coordinate vertical
* @param correctDistortions true for lens distortion correction, false otherwise
* @return disparity for the point on the plane specified by norm_xyz and known view coordinates px, py
*/
public double getPlaneDisparity(
double [] norm_xyz,
double px,
double py,
boolean correctDistortions) // correct distortion (will need corrected background too !)
{
double pXcd = px - 0.5 * this.pixelCorrectionWidth;
double pYcd = py - 0.5 * this.pixelCorrectionHeight;
double rD = Math.sqrt(pXcd*pXcd + pYcd*pYcd)*0.001*this.pixelSize; // distorted radius in a virtual center camera
double rND2R = correctDistortions?(getRByRDist(rD/this.distortionRadius, false)): 1.0;
double pXc = pXcd * rND2R; // non-distorted coordinates relative to the (0.5 * this.pixelCorrectionWidth, 0.5 * this.pixelCorrectionHeight)
double pYc = pYcd * rND2R; // in pixels
// point for the unity disparity
double x = SCENE_UNITS_SCALE * pXc * this.disparityRadius;
double y = -SCENE_UNITS_SCALE * pYc * this.disparityRadius;
double z = -SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / (0.001*this.pixelSize); // "+" - near, "-" far
double vect_dot_norm = (x * norm_xyz[0]) + (y * norm_xyz[1]) + (z * norm_xyz[2]);
double norm_dot_norm = (norm_xyz[0] * norm_xyz[0]) + (norm_xyz[1] * norm_xyz[1]) + (norm_xyz[2] * norm_xyz[2]);
return vect_dot_norm / norm_dot_norm;
}
/* Just for testing using delta instead of d */
public double [][] getWorldJacobian(
double px,
......@@ -419,15 +449,6 @@ public class GeometryCorrection {
// k = rD/r
double d_k_d_rrND = correctDistortions?getDerivRDistFromR(rrND):0.0;
double d_rND2R_d_rrD = - rND2R * rND2R * d_k_d_rrND / ( d_k_d_rrND * rrND + 1.0/ rND2R); // rrND);
/*
double d_rND2R_d_rrD0 = correctDistortions?(getDerivRByRDist(rrD, false)): 0.0;
double d_rND2R_d_rrD1 = correctDistortions?(getDerivRByRDist(rrD, false, 0.00001)): 0.0;
if (debug) {
System.out.println("getWorldJacobian(): d_rND2R_d_rrD="+d_rND2R_d_rrD+", d_rND2R_d_rrD0="+d_rND2R_d_rrD0+", d_rND2R_d_rrD1="+d_rND2R_d_rrD1 );
}
*/
double d_rND2R_d_px = d_rND2R_d_rrD * d_rRD_d_px;
double d_rND2R_d_py = d_rND2R_d_rrD * d_rRD_d_py;
......@@ -514,7 +535,7 @@ public class GeometryCorrection {
boolean correctDistortions,
int debugLevel)
{
if (debugLevel > 0){
if (debugLevel > 1){
System.out.println("getImageJacobian():");
}
double x = xyz[0];
......
src/main/java/SuperTiles.java
View file @ 1361f670
......@@ -21,8 +21,13 @@
** -----------------------------------------------------------------------------**
**
*/
import java.util.ArrayList;
import java.util.concurrent.atomic.AtomicInteger;
//import TilePlanes.PlaneData;
//import TilePlanes.PlaneData;
public class SuperTiles{
TileProcessor tileProcessor;
double step_far;
......@@ -45,6 +50,8 @@ public class SuperTiles{
double [] bgStrength = null;
// TileProcessor.CLTPass3d cltPass3d;
CLTPass3d cltPass3d;
// ArrayList<TilePlanes.PlaneData>[] planes = null;
TilePlanes.PlaneData [][] planes = null;
public SuperTiles(
CLTPass3d cltPass3d,
double step_near,
......@@ -118,7 +125,10 @@ public class SuperTiles{
return min_disparity + d;
}
public TilePlanes.PlaneData [][] getPlanes()
{
return planes;
}
private double [][] getLapWeights(){
final int superTileSize = tileProcessor.superTileSize;
final double [][] lapWeight = new double [2 * superTileSize][2 * superTileSize];
......@@ -1023,8 +1033,8 @@ public class SuperTiles{
final double min_disp,
final boolean invert_disp, // use 1/disparity
final double plDispNorm,
final int plMinPoints, // = 5; // Minimal number of points for plane detection
final double plTargetEigen, // = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
final int plMinPoints, // = 5; // Minimal number of points for plane detection
final double plTargetEigen, // = 0.1; // Remove outliers until main axis eigenvalue (possibly scaled by plDispNorm) gets below
final double plFractOutliers, // = 0.3; // Maximal fraction of outliers to remove
final int plMaxOutliers, // = 20; // Maximal number of outliers to remove
final GeometryCorrection geometryCorrection,
......@@ -1051,7 +1061,10 @@ public class SuperTiles{
final int len2 = superTileSize2*superTileSize2;
final double [] double_zero = new double [len2];
final boolean [] boolean_zero = new boolean [len2];
//ArrayList<Individual>[] group = (ArrayList<Individual>[])new ArrayList[4];
// this.planes = (ArrayList<TilePlanes.PlaneData>[]) new ArrayList[nStiles];
this.planes = new TilePlanes.PlaneData[nStiles][];
// final int debug_stile = 18 * stilesX + 25;
// final int debug_stile = 20 * stilesX + 24;
final int debug_stile = 17 * stilesX + 10;
......@@ -1102,8 +1115,9 @@ public class SuperTiles{
}
}
}
planes[nsTile] = null;
if (sw >0){
ArrayList<TilePlanes.PlaneData> st_planes = new ArrayList<TilePlanes.PlaneData>();
// 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;
......@@ -1124,7 +1138,7 @@ public class SuperTiles{
//correct_distortions
double swc_common = pd.getWeight();
if (dl > 0) {
if (swc_common > 1.0) {
if (swc_common > 0.3) { // 1.0) {
System.out.println("Processing planes["+nsTile+"]"+
", stileX="+stileX+
", stileY="+stileY+
......@@ -1157,7 +1171,7 @@ public class SuperTiles{
plMinPoints, // int minLeft, // minimal number of tiles to keep
dl1); // 0); // debugLevel);
if (dl > 0) {
if (swc_common > 1.0) {
if (swc_common > 0.3) { // 1.0) {
System.out.println("Removed outliers["+nsTile+"]"+
", stileX="+stileX+
", stileY="+stileY+
......@@ -1182,7 +1196,8 @@ public class SuperTiles{
norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
}
st_planes.add(pd);
// now try for each of the disparity-separated clusters
double [][] mm = maxMinMax[nsTile];
......@@ -1274,6 +1289,7 @@ public class SuperTiles{
}
norm_xyz = pd.getWorldXYZ(
correct_distortions);
st_planes.add(pd);
if (dl > 0) {
System.out.println("World normal["+nsTile+"]["+m+"] = {"+
norm_xyz[0]+", "+norm_xyz[1]+", "+norm_xyz[2]+"}");
......@@ -1283,7 +1299,11 @@ public class SuperTiles{
}
}
}
}
if (st_planes.size() > 0){
planes[nsTile] = st_planes.toArray(new TilePlanes.PlaneData[0] );
}
} // if sw >0
}
}
};
......@@ -1291,5 +1311,170 @@ public class SuperTiles{
ImageDtt.startAndJoin(threads);
}
public int [] getShowPlanesWidthHeight()
{
final int tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
// final int tileSize = tileProcessor.getTileSize();
final int stilesX = (tilesX + superTileSize -1)/superTileSize;
final int stilesY = (tilesY + superTileSize -1)/superTileSize;
int [] wh = {
stilesX * superTileSize,// * tileSize,
stilesY * superTileSize};// * tileSize};
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 tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
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;
for (int sty0 = 0; sty0 < stilesY; sty0++){
int sty = sty0 + dirsYX[dir][0];
for (int stx0 = 0; stx0 < stilesX; stx0++){
int nsTile0 = sty0 * stilesX + stx0; // result tile
int stx = stx0 + dirsYX[dir][1];
int nsTile = sty * stilesX + stx; // from where to get
if ((sty >= 0) && (sty < stilesY) && (stx >= 0) && (stx < stilesX) && (planes[nsTile] != null)) {
neib_planes[nsTile0] = new TilePlanes.PlaneData[planes[nsTile].length];
for (int np = 0; np < planes[nsTile].length; np++){
GeometryCorrection geometryCorrection = planes[nsTile][np].getGeometryCorrection();
int [] sTileXY0 = {stx0,sty0};
if (tpl == null) {
tpl = new TilePlanes(tileSize,superTileSize, geometryCorrection);
}
TilePlanes.PlaneData pd = tpl.new PlaneData(
sTileXY0,
tileSize,
superTileSize,
geometryCorrection);
/*
TilePlanes.PlaneData pd = planes[nsTile0][np].clone();
pd.setS
*/
if (nsTile0 == debug_stile) {
System.out.println("getNeibPlanes(): nsTile0="+nsTile0+", nsTile="+nsTile+", np = "+np+" dir = "+ dir );
}
neib_planes[nsTile0][np] = pd.getPlaneToThis(
planes[nsTile][np], // PlaneData otherPd,
(nsTile0 == debug_stile)? 1:0); // int debugLevel)
}
} else {
neib_planes[nsTile0] = null;
}
}
}
return neib_planes;
}
public double [][] getShowPlanes(
TilePlanes.PlaneData [][] planes,
double minWeight,
double maxEigen,
double dispNorm,
boolean use_NaN)
{
final int tilesX = tileProcessor.getTilesX();
final int tilesY = tileProcessor.getTilesY();
final int superTileSize = tileProcessor.getSuperTileSize();
// 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 debug_stile = 17 * stilesX + 27;
final int debug_stile = 20 * stilesX + 27;
// final int debug_stile = 19 * stilesX + 27;
// scan all get maximal number of satisfying planes
int num_planes = 0;
for (int i = 0; i < planes.length; i++) {
if (i == debug_stile) {
System.out.println("getShowPlanes():1, tile = "+i);
}
if ((planes[i] != null) && (planes[i].length > num_planes)){
int num_sat = 0;
for (int j = 0; j < planes[i].length; j++){
if (planes[i][j].getWeight() >= minWeight){
double eigVal = planes[i][j].getValue(); // ************** generated does not have value ????????????
double disp = planes[i][j].getPlaneDisparity(false)[centerIndex];
if (disp > dispNorm) {
eigVal *= dispNorm / disp;
}
if (eigVal < maxEigen) num_sat ++;
}
}
if (num_sat > num_planes) num_planes = num_sat;
}
}
double [][] data = new double[num_planes][width*height];
for (int sty = 0; sty < stilesY; sty++){
for (int stx = 0; stx < stilesX; stx++){
int nsTile = sty * stilesX + stx;
if (nsTile == debug_stile) {
System.out.println("getShowPlanes():2, tile = "+nsTile);
}
int ns = 0;
double [] plane = nan_plane;
int indx = (sty * superTileSize) * width + (stx * superTileSize);
boolean debug = (nsTile == debug_stile) ; // (sty == 18) && (stx == 28);
if (debug) {
System.out.println("getShowPlanes():"+((planes[nsTile] != null)? planes[nsTile].length : "null"));
}
if (planes[nsTile] != null) {
for (int np = 0; np < planes[nsTile].length; np++){
if (planes[nsTile][np].getWeight() >= minWeight){
//&& (planes[nsTile][np].getValue() < maxEigen)){
double eigVal = planes[nsTile][np].getValue();
// double disp = planes[nsTile][np].getPlaneDisparity(false)[centerIndex];
double disp = planes[nsTile][np].getZxy()[0];
if (disp > dispNorm) {
eigVal *= dispNorm / disp;
}
if (eigVal < maxEigen) {
plane = planes[nsTile][np].getPlaneDisparity(use_NaN);
for (int y = 0; y < superTileSize; y++) {
System.arraycopy(plane, superTileSize * y, data[ns], indx + width * y, superTileSize);
}
ns ++;
}
}
}
}
// fill same (nearest) plane or NaN if none was available
for (; ns < num_planes; ns++){
for (int y = 0; y < superTileSize; y++) {
System.arraycopy(plane, superTileSize * y, data[ns], indx + width * y, superTileSize);
}
}
}
}
return data;
}
} // end of class SuperTiles
src/main/java/TilePlanes.java
View file @ 1361f670
......@@ -46,17 +46,47 @@ public class TilePlanes {
public class PlaneData{
GeometryCorrection geometryCorrection = null;
boolean correctDistortions = false;
double [] zxy; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
boolean [] plane_sel = null; // tile selection - has twice supertile size in each direction
double [] zxy = null; // [3] - plane center point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile (in pixels, relative to this supertile center)
double [][] vectors = null; // [3][3] - re-ordered/re-directed eigenvectors(transposed): [0] - plane normal, most Z-like, towards camera, [1] - X-like, [2] - Y-like
double [] values = null; // [3] -eigenvalues
int num_points = 0;
double weight = 0.0;
double [] center_xyz = null; // center of this supertile this plane center in world coordinates
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
int tileSize;
int superTileSize;
int [] sTileXY = null; // X and Y indices of this superTile in the image
public PlaneData clone(){
PlaneData pd = new PlaneData(
this.sTileXY,
this.tileSize,
this.superTileSize,
this.geometryCorrection);
pd.num_points = this.num_points;
pd.weight = this.weight;
if (this.plane_sel != null) pd.plane_sel = this.plane_sel.clone();
if (this.zxy != null) pd.zxy = this.zxy.clone();
if (this.values != null) pd.values = this.values.clone();
if (this.center_xyz != null) pd.center_xyz = this.center_xyz.clone();
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.vectors != null) {
pd.vectors = new double[3][];
pd.vectors[0] = this.vectors[0].clone();
pd.vectors[1] = this.vectors[1].clone();
pd.vectors[2] = this.vectors[2].clone();
}
return pd;
}
public PlaneData (
int [] sTileXY,
int tileSize,
......@@ -76,15 +106,26 @@ public class TilePlanes {
public boolean getCorrectDistortions() {
return correctDistortions;
}
public boolean [] getPlaneSelection(){
return plane_sel;
}
public void setPlaneSelection(boolean [] plane_sel){
this.plane_sel = plane_sel;
}
public int[] getSTileXY() {
return sTileXY;
}
public double[] getZxy() {
return zxy;
}
public void setZxy(double[] zxy) {
this.zxy = zxy;
}
public double[][] getVectors() {
return vectors;
}
......@@ -125,6 +166,56 @@ public class TilePlanes {
{
return getWorldXYZ(correct_distortions,0);
}
public double [] getCenterPxPy() // supertile center (not plane center)
{
double [] px_py = {
tileSize*(superTileSize * sTileXY[0] + superTileSize/2), // + zxy[1],
tileSize*(superTileSize * sTileXY[1] + superTileSize/2)}; // + zxy[2]};
return px_py;
}
public GeometryCorrection getGeometryCorrection() {
return geometryCorrection;
}
public void setGeometryCorrection(GeometryCorrection geometryCorrection) {
this.geometryCorrection = geometryCorrection;
}
/**
* Get disparity values for the tiles of this supertile as [superTileSize*superTileSize] array
* @param useNaN replace unselected tiles with Double.NaN
* @return array of disparity values for the plane (not including overlapped areas)
*/
public double[] getPlaneDisparity(
boolean useNaN)
{
double [] disparities = new double[superTileSize*superTileSize];
int indx = 0;
double [] normal = getVector();
double [] zxy = getZxy(); // {disparity, x center in pixels, y center in pixels (relative to a supertile center)
for (int sy = -superTileSize/2; sy < superTileSize/2; sy++){
int indx_sel = (2*sy + superTileSize) * superTileSize + superTileSize/2;
double y = tileSize * sy - zxy[2];
for (int sx = -superTileSize/2; sx < superTileSize/2; sx++){
double x = tileSize * sx - zxy[1];
if (plane_sel[indx_sel] || !useNaN || (plane_sel==null)){
disparities[indx] = zxy[0] - (normal[1] * x + normal[2] * y)/normal[0];
} else {
disparities[indx] = Double.NaN;
}
indx++;
indx_sel++;
}
}
return disparities;
}
// double px = tileSize*(superTileSize * sTileXY[0] + superTileSize/2) + zxy[1]; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
// double py = tileSize*(superTileSize * sTileXY[1] + superTileSize/2) + zxy[2];
/**
* Cross product of 2 3-d vectors as column matrices
* @param v1
......@@ -142,6 +233,162 @@ public class TilePlanes {
return new Matrix (ar);
}
/**
* 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
* calculated, two other axes preserve just px/py ratio.
* Transformed center is calculated for original px, py of the center and recalculated plane to get disparity value
* The converted plane data can be combined with the current one to get plane data for combined tiles.
* Converted disparity is not limited to positive values.
* @param otherPd other supertile plane data (will not be modified)
* @return plane data converted to local supertile disparity, px, py (center and eigenvectors)
*/
public PlaneData getPlaneToThis(
PlaneData otherPd,
int debugLevel)
{
PlaneData pd = otherPd.clone();
if (debugLevel > 0) {
System.out.println("getPlaneToThis()");
}
double [] px_py = getCenterPxPy(); // this supertile center
double [] px_py_other = pd.getCenterPxPy(); // other supertile center
double [] wv1 = otherPd.getWorldV12( // 1-st in-plane vector will calculate, but that will not be used below?
false, // false - first, true - second vector
this.correctDistortions);
double [] wv2 = otherPd.getWorldV12( // 2-nd in-plane vector
true, // false - first, true - second vector
this.correctDistortions);
double disp = geometryCorrection.getPlaneDisparity( // disparity (at this center) for crossing other supertile plane
pd.getWorldXYZ(this.correctDistortions), // will calculate if not yet done so. Should it use otherPd, not pd? and then clone later?
px_py[0],
px_py[1],
this.correctDistortions);
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), px_py = {"+px_py[0]+", "+px_py[1]+"}, px_py_other = {"+px_py_other[0]+", "+px_py_other[1]+"}");
System.out.println("getPlaneToThis(), disp = "+disp);
System.out.println("getPlaneToThis(), wv1 = {"+ wv1[0]+", "+ wv1[1]+", "+ wv1[2]+"}");
System.out.println("getPlaneToThis(), wv2 = {"+ wv2[0]+", "+ wv2[1]+", "+ wv2[2]+"}");
}
// find world coodinates of the center of tile intersection with the plane
Matrix xyz = new Matrix(geometryCorrection.getWorldCoordinates(
px_py[0],
px_py[1],
disp,
this.correctDistortions),3);
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), Matrix xyz=");
xyz.print(10,6);
}
// Jacobian to convert world vectors to
Matrix img_jacobian = new Matrix(geometryCorrection.getImageJacobian(
xyz.getColumnPackedCopy(),
this.correctDistortions,
1)); // debug_level
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), Matrix img_jacobian=");
img_jacobian.print(10,6);
}
// 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
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), Matrix v0 =");
v0.print(10,6);
System.out.println("getPlaneToThis(), Matrix v1 =");
v1.print(10,6);
System.out.println("getPlaneToThis(), Matrix v2 =");
v2.print(10,6);
}
// normalize v0, update v1, v2 to be orthonormal with v0
v0.timesEquals(1.0/v0.normF()); // unity vector;
v1.minusEquals(v0.times(v0.transpose().times(v1)));
v1.timesEquals(1.0/v1.normF()); // unity vector;
v2.minusEquals(v0.times(v0.transpose().times(v2)));
v2.minusEquals(v1.times(v1.transpose().times(v2)));
v2.timesEquals(1.0/v2.normF()); // unity vector;
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), ortho-normalized Matrix v0 =");
v0.print(10,6);
System.out.println("getPlaneToThis(), ortho-normalized Matrix v1 =");
v1.print(10,6);
System.out.println("getPlaneToThis(), ortho-normalized Matrix v2 =");
v2.print(10,6);
}
pd.vectors[0] = v0.getColumnPackedCopy();
pd.vectors[1] = v1.getColumnPackedCopy();
pd.vectors[2] = v2.getColumnPackedCopy();
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), pd.vectors[0] = {"+ pd.vectors[0][0]+", "+ pd.vectors[0][1]+", "+ pd.vectors[0][2]+"}");
System.out.println("getPlaneToThis(), pd.vectors[1] = {"+ pd.vectors[1][0]+", "+ pd.vectors[1][1]+", "+ pd.vectors[1][2]+"}");
System.out.println("getPlaneToThis(), pd.vectors[2] = {"+ pd.vectors[2][0]+", "+ pd.vectors[2][1]+", "+ pd.vectors[2][2]+"}");
}
// double [] dxy = otherPd.getZxy(); // {disparity, px, py} px, py in pixels, relative to the other supertile center
double [] dxy = pd.getZxy(); // {disparity, px, py} px, py in pixels, relative to the other supertile center
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), otherPd.getZxy() = {"+ dxy[0]+", "+ dxy[1]+", "+ dxy[2]+"}");
}
dxy[1] += px_py_other[0] - px_py[0]; // relative to this tile
dxy[2] += px_py_other[1] - px_py[1]; // relative to this tile
// find center of plane segment - get intersection of the plane orthogonal to v0 through the point px_py to the point pd.
dxy[0] = disp - (pd.vectors[0][1]*dxy[1] + pd.vectors[0][2]*dxy[2]) / pd.vectors[0][0];
if (debugLevel > 0) {
System.out.println("getPlaneToThis(), dxy(modified) = {"+ dxy[0]+", "+ dxy[1]+", "+ dxy[2]+"}");
}
pd.sTileXY = this.sTileXY;
return pd; // make sure pd are updated // "this" is not used. Should it be used instead of pd?
}
public double [] getWorldV12(
boolean v2, // false - first, true - second vector
boolean correct_distortions)
{
return getWorldV12(v2, correct_distortions,0);
}
public double [] getWorldV12(
boolean v2, // false - first, true - second vector
boolean correct_distortions,
int debugLevel)
{
if (v2) {
if (world_v2 == null){
getWorldXYZ(
correct_distortions,
debugLevel);
}
return world_v2;
} else {
if (world_v1 == null){
getWorldXYZ(
correct_distortions,
debugLevel);
}
return world_v1;
}
}
public double [] getWorldXYZ(
boolean correct_distortions,
int debugLevel)
......@@ -150,18 +397,27 @@ public class TilePlanes {
if (world_xyz != null) return world_xyz;
setCorrectDistortions(correct_distortions);
// get pixel coordinates of the plane origin point
double px = tileSize*(superTileSize * sTileXY[0] + superTileSize/2) + zxy[1]; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
double py = tileSize*(superTileSize * sTileXY[1] + superTileSize/2) + zxy[2];
// double px = tileSize*(superTileSize * sTileXY[0] + superTileSize/2) + zxy[1]; // [3] - plane point {disparity, x, y), x=0, y=0 is a 4,4 point of an 8x8 supertile
// double py = tileSize*(superTileSize * sTileXY[1] + superTileSize/2) + zxy[2];
double [] px_py = getCenterPxPy();
if ((px_py[0] == 1760) && (px_py[1] == 1056)){ // 27, 15
System.out.println("getWorldXYZ, px_py = {"+px_py[0]+","+px_py[1]+"}");
debugLevel = 2;
}
double px = px_py[0] + zxy[1];
double py = px_py[1] + zxy[2];
double disp = zxy[0];
Matrix xyz = new Matrix(geometryCorrection.getWorldCoordinates(
center_xyz = geometryCorrection.getWorldCoordinates(
px,
py,
disp,
this.correctDistortions),3); // column matrix
this.correctDistortions);
Matrix xyz = new Matrix(center_xyz, 3); // column matrix
Matrix dpxpy = new Matrix(vectors[0],3); // 3 rows, 1 column
if (debugLevel > 0){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"), correctDistortions="+correctDistortions+", xyz= {"+
xyz.get(0, 0)+","+xyz.get(1, 0)+","+xyz.get(2, 0)+"}");
xyz.get(0, 0)+","+xyz.get(1, 0)+","+xyz.get(2, 0)+"}, weight = "+getWeight());
// xyz.print(10, 6); // w,d
// double [] dpxpy = geometryCorrection.getImageCoordinates(xyz.getColumnPackedCopy(),this.correctDistortions);
......@@ -188,6 +444,10 @@ public class TilePlanes {
// convert both orthogonal axes, normalize their cross product
Matrix v1 = jacobian.times(new Matrix(vectors[1],3)); // 3 rows, 1 column
Matrix v2 = jacobian.times(new Matrix(vectors[2],3)); // 3 rows, 1 column
world_v1 = v1.getColumnPackedCopy();
world_v2 = v2.getColumnPackedCopy();
Matrix norm_xyz = cross3d(v1,v2);
// norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
// norm_xyz = jacobian.times(dpxpy); // plane normal vector in world xyz
......@@ -201,16 +461,6 @@ public class TilePlanes {
dpxpy.print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian=");
jacobian.print(10, 6); // w,d
/*
Matrix jacobian1 = new Matrix(geometryCorrection.getWorldJacobian(
px,
py,
disp,
this.correctDistortions,
0.00001));
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1=");
jacobian1.print(10, 6); // w,d
*/
}
if (debugLevel > 2){
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): norm_xyz=");
......@@ -223,35 +473,20 @@ public class TilePlanes {
this.correctDistortions,
1));
img_jacobian.print(10, 6); // w,d
/*
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image jacobian1 (0.000001)=");
Matrix img_jacobian1 = new Matrix(geometryCorrection.getImageJacobian(
xyz.getColumnPackedCopy(),
this.correctDistortions,
0.000001));
img_jacobian1.print(10, 6); // w,d
*/
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian)=");
jacobian.times(img_jacobian).print(10, 6); // w,d
/*
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian.times(image_jacobian1)=");
jacobian.times(img_jacobian1).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian)=");
jacobian1.times(img_jacobian).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): jacobian1.times(image_jacobian1)=");
jacobian1.times(img_jacobian1).print(10, 6); // w,d
System.out.println("getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): image_jacobian.inverse()=");
img_jacobian.inverse().print(10, 6); // w,d
*/
}
norm_xyz = norm_xyz.times(1.0/norm_xyz.normF()); // unity normal vector;
norm_xyz.timesEquals(1.0/norm_xyz.normF()); // unity normal vector;
if (debugLevel > 0){
System.out.println("+getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): unit plane normal={"+
norm_xyz.get(0, 0)+", "+norm_xyz.get(1, 0)+", "+norm_xyz.get(2, 0)+"})");
double dotprod = xyz.transpose().times(norm_xyz).get(0,0);
// Matrix wn = norm_xyz.times(-dotprod);
Matrix wn = norm_xyz.times(dotprod);
System.out.println(":getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): xyz.transpose().times(norm_xyz).get(0,0) ="+dotprod);
System.out.println("?getWorldXYZ("+sTileXY[0]+","+sTileXY[1]+"): plane normal={"+
wn.get(0, 0)+", "+wn.get(1, 0)+", "+wn.get(2, 0)+"})");
}
......@@ -263,7 +498,8 @@ public class TilePlanes {
// convert plane normal vector to world coordinates
//world_xyz
world_xyz = norm_xyz.times(-(xyz.times(norm_xyz.transpose()).get(0,0))).getColumnPackedCopy();
// world_xyz = norm_xyz.times(-(xyz.transpose().times(norm_xyz).get(0,0))).getColumnPackedCopy();
world_xyz = norm_xyz.times((xyz.transpose().times(norm_xyz).get(0,0))).getColumnPackedCopy();
return world_xyz;
}
}
......@@ -356,8 +592,12 @@ public class TilePlanes {
int [] sTileXY,
double [] data,
double [] weight,
boolean [] select,
boolean [] select, // null OK, will enable all tiles
int debugLevel){
if (select == null) {
select = new boolean [4*stSize];
for (int i = 0; i < select.length; i++) select[i] = true;
}
double [][][] rslt = getCovar(
data,
weight,
......@@ -372,13 +612,14 @@ public class TilePlanes {
double [][] eig_val = rslt[0];
double [][] eig_vect = rslt[1];
// find vector most orthogonal to view // (anyway it all works with that assumption), make it first
// TODO?
// TODO normalize to local linear scales
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;
}
}
/*
// Find two other axis - "mostly X" (horizontal) and "mostly Y" (vertical)
int vindx = (oindx == 0)? 1 : 0;
int hindx = (oindx == 0)? 2 : ((oindx == 1) ? 2 : 1);
......@@ -387,6 +628,16 @@ public class TilePlanes {
vindx = hindx;
hindx = tmp;
}
*/
// 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;
}
PlaneData pd = new PlaneData(
sTileXY,
this.tileSize,
......@@ -400,13 +651,26 @@ public class TilePlanes {
{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), last one - in positive Y (down)
for (int v = 0; v <3; v++) {
if (plane[v][v] < 0.0) for (int i = 0; i < 3; i ++) plane[v][i] = -plane[v][i];
}
*/
// 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];
}
pd.setVectors (plane);
pd.setNumPoints (numPoints);
pd.setWeight (swc);
pd.setPlaneSelection(select);
return pd;
}
......@@ -426,8 +690,10 @@ public class TilePlanes {
sTileXY,
data,
weight,
select,
select, // null OK
debugLevel);
} else if (select != null){
pd.setPlaneSelection(select);
}
if (maxRemoved > (pd.getNumPoints() - minLeft)) maxRemoved = pd.getNumPoints() - minLeft;
int numRemoved = 0;
......
src/main/java/TileProcessor.java
View file @ 1361f670
......@@ -2973,6 +2973,58 @@ public class TileProcessor {
geometryCorrection,
clt_parameters.correct_distortions,
debugLevel); // final int debugLevel)
if (clt_parameters.show_planes){
int [] wh = st.getShowPlanesWidthHeight();
double [][] plane_data_nonan = st.getShowPlanes(
st.getPlanes(),
clt_parameters.plMinStrength, // minWeight,
clt_parameters.plMaxEigen, // maxEigen,
clt_parameters.plDispNorm,
false); //boolean use_NaN)
double [][] plane_data_nan = st.getShowPlanes(
st.getPlanes(),
clt_parameters.plMinStrength, // minWeight,
clt_parameters.plMaxEigen, // maxEigen,
clt_parameters.plDispNorm,
true); //boolean use_NaN)
double [][] plane_data = new double [plane_data_nonan.length + plane_data_nan.length][];
int indx = 0;
for (int i = 0; i < plane_data_nonan.length; i++){
plane_data[indx++] = plane_data_nonan[i];
}
for (int i = 0; i < plane_data_nan.length; i++){
plane_data[indx++] = plane_data_nan[i];
}
// sdfa_instance.showArrays(plane_data_nonan, wh[0], wh[1], true, "plane_data_noNaN");
// sdfa_instance.showArrays(plane_data_nan, wh[0], wh[1], true, "plane_data_NaN");
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);
plane_data_nonan = st.getShowPlanes(
planes,
clt_parameters.plMinStrength, // minWeight,
clt_parameters.plMaxEigen, // maxEigen,
clt_parameters.plDispNorm,
false); //boolean use_NaN)
plane_data_nan = st.getShowPlanes(
planes,
clt_parameters.plMinStrength, // minWeight,
clt_parameters.plMaxEigen, // maxEigen,
clt_parameters.plDispNorm,
true); //boolean use_NaN)
plane_data = new double [plane_data_nonan.length + plane_data_nan.length][];
indx = 0;
for (int i = 0; i < plane_data_nonan.length; i++){
plane_data[indx++] = plane_data_nonan[i];
}
for (int i = 0; i < plane_data_nan.length; i++){
plane_data[indx++] = plane_data_nan[i];
}
sdfa_instance.showArrays(plane_data, wh[0], wh[1], true, "plane_data_"+dr);
}
}
/*
st.processPlanes1(
null, // final boolean [] selected, // or null
......@@ -3001,7 +3053,7 @@ public class TileProcessor {
dbg_with_super_disp = scan_prev.combineSuper(
true, //boolean updateStrength, // use ST strength if true, keep original (update disparity only) if false
clt_parameters.stStrengthScale, // Multiply st strength if used instead of regular strength (only if updateStrength)
clt_parameters.stUseDisp); //.15; // Use background disparity from supertiles if tile strength is less
clt_parameters.stUseDisp); //.15; // Use background disparity from supertiles if tile strength is less
if (dbg_with_super_disp != null) dbg_with_super_disp = dbg_with_super_disp.clone(); // else no super disparity available
......
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