Skip to content

I
imagej-elphel
Commit 9982a7ba authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

fixing ground planes

parent 009eb8ea
Show whitespace changes
Inline Side-by-side
Showing with 123 additions and 145 deletions
src/main/java/com/elphel/imagej/tileprocessor/GroundPlane.java
View file @ 9982a7ba
...@@ -546,27 +546,6 @@ public class GroundPlane { ...@@ -546,27 +546,6 @@ public class GroundPlane {
double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]}); double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz); double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
//??? seems opposite rotation
virtual_camera_from_camera[1] = to_ground_xyzatr[1];
/*
double [][] to_ground_xyz = {to_ground_xyzatr[0],new double[3]};
double [][] to_ground_atr = {new double[3], to_ground_xyzatr[1]};
double [][] from_ground_xyz = ErsCorrection.invertXYZATR(to_ground_xyz);
// calculate rotated around ground relative to the reference frame (see if z-sign is correct, or swap to/from
//combineXYZATR
double [][] to_ground_xyzatr1 = ErsCorrection.combineXYZATR(to_ground_xyz, ErsCorrection.invertXYZATR(to_ground_atr));
/// double [][] to_ground_xyzatr1 = ErsCorrection.combineXYZATR(to_ground_atr,to_ground_xyz);
double [][] to_ground_xyzatr_centered = ErsCorrection.combineXYZATR(to_ground_xyzatr1, from_ground_xyz);
/// double [][] to_ground_xyzatr_centered = ErsCorrection.combineXYZATR(from_ground_xyz, to_ground_xyzatr1);
double [][] to_ground_xyzatr_centered1 = ErsCorrection.combineXYZATR(from_ground_xyz,
ErsCorrection.combineXYZATR(to_ground_atr, to_ground_xyz));
to_ground_xyzatr_centered = new double [][] {
{-to_ground_xyzatr_centered[0][0],-to_ground_xyzatr_centered[0][1],-to_ground_xyzatr_centered[0][2]},
to_ground_xyzatr_centered[1]
};
*/
if ((dbg_title != null) || (gnd_disp != null)) { if ((dbg_title != null) || (gnd_disp != null)) {
// expand macrotiles results to match tiles // expand macrotiles results to match tiles
// rotate ref_disparity and show // rotate ref_disparity and show
...@@ -588,7 +567,6 @@ public class GroundPlane { ...@@ -588,7 +567,6 @@ public class GroundPlane {
int tilesY = wxyz.length/width; int tilesY = wxyz.length/width;
int mtile_half = mtile_size/2; int mtile_half = mtile_size/2;
int mtilesX = (tilesX - mtile_size)/mtile_half + 1; int mtilesX = (tilesX - mtile_size)/mtile_half + 1;
// int mtilesY = (tilesY - mtile_size)/mtile_half + 1;
double [][] macro_exp = new double [tilesX * tilesY][]; double [][] macro_exp = new double [tilesX * tilesY][];
double [][] macro_wexp = new double [tilesX * tilesY][]; double [][] macro_wexp = new double [tilesX * tilesY][];
for (int mtile = 0; mtile < macroplanes.length; mtile++){ for (int mtile = 0; mtile < macroplanes.length; mtile++){
...@@ -691,16 +669,10 @@ public class GroundPlane { ...@@ -691,16 +669,10 @@ public class GroundPlane {
if (good_tiles != null) { if (good_tiles != null) {
System.arraycopy(macro_mask, 0, good_tiles, 0, macro_mask.length); System.arraycopy(macro_mask, 0, good_tiles, 0, macro_mask.length);
} }
ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered); ErsCorrection.printVectors(virtual_camera_from_camera); // to_ground_xyzatr_centered);
/// ref_Clt.getErsCorrection().printVectors (to_ground_xyzatr_centered[0], to_ground_xyzatr_centered[1]);
ref_Clt.getErsCorrection().printVectors (virtual_camera_from_camera[0], virtual_camera_from_camera[1]); ref_Clt.getErsCorrection().printVectors (virtual_camera_from_camera[0], virtual_camera_from_camera[1]);
//num_good2 //num_good2
/// return new double[][] {to_ground_xyzatr_centered[0],to_ground_xyzatr_centered[1], {1.0*num_good2/ref_disparity.length}};
// return new double[][] {virtual_camera_from_camera[0],virtual_camera_from_camera[1], {1.0*num_good2/ref_disparity.length}};
return new double[][] {to_ground_xyzatr[0], to_ground_xyzatr[1], {1.0*num_good2/ref_disparity.length}}; return new double[][] {to_ground_xyzatr[0], to_ground_xyzatr[1], {1.0*num_good2/ref_disparity.length}};
// return to_ground_xyzatr_centered;
} }
public static boolean [] getMaskFromMacro( public static boolean [] getMaskFromMacro(
...@@ -1141,7 +1113,7 @@ public class GroundPlane { ...@@ -1141,7 +1113,7 @@ public class GroundPlane {
int [] xy0 = {tilesX/2, tilesY/2}; int [] xy0 = {tilesX/2, tilesY/2};
double inf_disparity = ref_Clt.getDispInfinityRef(); double inf_disparity = ref_Clt.getDispInfinityRef();
double [] disparity_tilts = gp.getDisparityTilts(); double [] disparity_tilts = gp.getDisparityTilts();
if ((test_bottom >0) && (test_bottom < 1)) { if ((test_bottom > 0) && (test_bottom < 1)) {
int y1 = (int) ((terrain_plane.length/tilesX) * test_bottom); int y1 = (int) ((terrain_plane.length/tilesX) * test_bottom);
if (disparity_tilts != null) { if (disparity_tilts != null) {
for (int i = 0; i < terrain_plane.length; i++) { for (int i = 0; i < terrain_plane.length; i++) {
...@@ -1174,13 +1146,7 @@ public class GroundPlane { ...@@ -1174,13 +1146,7 @@ public class GroundPlane {
to_ground_xyzatr); // double [][] to_ground_xyzatr to_ground_xyzatr); // double [][] to_ground_xyzatr
double [][] stereo_xyzatr = ErsCorrection.invertXYZATR(virt_camera); double [][] stereo_xyzatr = ErsCorrection.invertXYZATR(virt_camera);
// double [][] center_rot_xyzatr = {new double [3],stereo_xyzatr[1]};
/*
double [] terrain_plane = rotateRefDisparity(
ref_Clt, // final QuadCLT ref_Clt,
terrain_plane, // double [] terrain,
stereo_xyzatr); // virt_camera); // final double [][] scene_xyzatr)
*/
ref_Clt.setTerrain( ref_Clt.setTerrain(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
terrain_plane, // double [] terrain, terrain_plane, // double [] terrain,
...@@ -1194,20 +1160,9 @@ public class GroundPlane { ...@@ -1194,20 +1160,9 @@ public class GroundPlane {
double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]}); double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz); double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
/// double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(virtual_camera_from_ground_xyz, ground_xyzatr); virtual_camera_from_camera[1] = to_ground_xyzatr[1]; // *******????????
// double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
virtual_camera_from_camera[1] = to_ground_xyzatr[1];
return virtual_camera_from_camera; return virtual_camera_from_camera;
} }
/*
double [][] ground_xyzatr = ErsCorrection.invertXYZATR(to_ground_xyzatr); // straight down from the camera, then rotated
double [][] virtual_camera_from_ground_xyz = ErsCorrection.invertXYZATR(new double [][] {ground_xyzatr[0],new double[3]});
double [][] virtual_camera_from_camera = ErsCorrection.combineXYZATR(ground_xyzatr, virtual_camera_from_ground_xyz);
//??? seems opposite rotation
virtual_camera_from_camera[1] = to_ground_xyzatr[1];
*/
public static double [] getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good public static double [] getPlaneDualPass( // returns tiltX, tiltY, disp_center, frac_good
final CLTParameters clt_parameters, final CLTParameters clt_parameters,
...@@ -1669,7 +1624,24 @@ public class GroundPlane { ...@@ -1669,7 +1624,24 @@ public class GroundPlane {
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]); System.out.println("Found ground plane: level="+z_tilts[0]+", tiltX="+z_tilts[1]+", tiltY="+z_tilts[2]);
} }
double [][] ground_xyzatr = new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}}; /// double [][] ground_xyzatr = new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{Math.asin(z_tilts[1]), -Math.asin(z_tilts[2]), 0.0}};
/// double [][] ground_xyzatr00 = new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{-Math.asin(z_tilts[1]), Math.asin(z_tilts[2]), 0.0}};
// or opposite order?
boolean invert_order = false;
if (debugLevel > -3) {
System.out.println("Using "+(invert_order?"inverted":"direct")+" azimuth/tilt order when convertingtiltX/tiltY to angles.");
}
double tl = 0, az=0;
if (invert_order) {
tl = -Math.atan(-z_tilts[2]/Math.sqrt(1+z_tilts[1]*z_tilts[1]));
az = -Math.atan(z_tilts[1]);
} else {
az = Math.atan(-z_tilts[1]/Math.sqrt(1+z_tilts[2]*z_tilts[2]));
tl = Math.atan(z_tilts[2]);
}
double [][] ground_xyzatr = new double [][] {{0, 0, use_parallel_proj?0:z_tilts[0]},{az, tl, 0.0}};
// It is approximate for small angles. OK for now // It is approximate for small angles. OK for now
double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(ground_xyzatr); double [][] to_ground_xyzatr = ErsCorrection.invertXYZATR(ground_xyzatr);
return to_ground_xyzatr; // from the camera coordinates to in-plane coordinates return to_ground_xyzatr; // from the camera coordinates to in-plane coordinates
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 9982a7ba
...@@ -7346,42 +7346,25 @@ public class OpticalFlow { ...@@ -7346,42 +7346,25 @@ public class OpticalFlow {
} }
boolean export_terrain_sequence = true; // check master_CLT boolean export_terrain_sequence = true; // check master_CLT
if (export_terrain_sequence && !clt_parameters.imp.lock_position) { if (export_terrain_sequence && !clt_parameters.imp.lock_position) {
final double test_bottom = 0.5; final double test_bottom = 0.0; // 0.5; // disable half-tilted
{ {
double [] stereo_offset = ZERO3; // No rotations orthogonal to ground, using tilted disparity
double [] stereo_atr = null; GroundPlane.prepareTerrainRender(
double [][] gnp = GroundPlane.prepareTerrainRender(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
master_CLT, // final QuadCLT ref_Clt, master_CLT, // final QuadCLT ref_Clt,
true, // final boolean tilted_plane, true, // final boolean tilted_plane,
test_bottom, // final double test_bottom, test_bottom, // final double test_bottom,
debugLevel); // final int debugLevel) debugLevel); // final int debugLevel)
double [][] ignp = null;
if (gnp != null) {
ignp = ErsCorrection.invertXYZATR(gnp);
// stereo_offset = new double [] {-ground_normal_pose[0][0],-ground_normal_pose[0][1],-ground_normal_pose[0][2]};
stereo_offset = gnp[0]; // stereo_pose[0];
stereo_atr = gnp[1]; // stereo_pose[1];
// stereo_atr = new double [] {-ignp[1][0],-ignp[1][1],-ignp[1][2]}; // stereo_pose[1];
if (debugLevel > -3) {
System.out.println("Using airplane mode terrane as a single horizontal plane");
System.out.println("stereo_offset = ["+stereo_offset[0]+", "+stereo_offset[1]+", "+stereo_offset[2]+"]");
System.out.println("stereo_atr = ["+stereo_atr[0]+ ", "+stereo_atr[1]+ ", "+stereo_atr[2]+ "]");
// TODO add provisions to the non-flat terrain -modify terrain slice, keeping ground_normal_pose as a horizontal plane.
System.out.println("ignp[0] = ["+ignp[0][0]+", "+ignp[0][1]+", "+ignp[0][2]+"]");
System.out.println("ignp[1] = ["+ignp[1][0]+", "+ignp[1][1]+", "+ignp[1][2]+"]");
}
}
combo_dsn_final =master_CLT.restoreComboDSI(true); combo_dsn_final =master_CLT.restoreComboDSI(true);
if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) { if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) {
System.out.println ("No terrain data available"); System.out.println ("No terrain data available");
} else { } else {
double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN]; double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN];
String scenes_suffix_disp = master_CLT.getImageName()+"-TERRAIN-DISP"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN"; String scenes_suffix_disp = master_CLT.getImageName()+"-TERRAIN-DISP";
ImagePlus imp_terrain_disp = renderSceneSequence( ImagePlus imp_terrain_disp = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas, false, // master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono, false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average, clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice, null, // float [] average_slice,
...@@ -7402,39 +7385,11 @@ public class OpticalFlow { ...@@ -7402,39 +7385,11 @@ public class OpticalFlow {
master_CLT.saveImagePlusInModelDirectory( master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_disp); // imp_scenes); // ImagePlus imp) imp_terrain_disp); // imp_scenes); // ImagePlus imp)
String scenes_suffix_test = master_CLT.getImageName()+"-TERRAIN-TEST2"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_test = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
true, // master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // gnp[0], // double [] stereo_offset, // offset reference camera {x,y,z}
null, // gnp[1], // double [] stereo_atr, // offset reference orientation (cuas)
gnp, // new double [][] {stereo_offset,stereo_atr}, //
1, // int sensor_mask,
scenes_suffix_test, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_test); // imp_scenes); // ImagePlus imp)
} }
} }
{ {
double [] stereo_offset = ZERO3; // rotating orthogonal to ground, using constant disparity
double [] stereo_atr = null;
double [][] gnp = GroundPlane.prepareTerrainRender( double [][] gnp = GroundPlane.prepareTerrainRender(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, // final CLTParameters clt_parameters,
master_CLT, // final QuadCLT ref_Clt, master_CLT, // final QuadCLT ref_Clt,
...@@ -7444,14 +7399,10 @@ public class OpticalFlow { ...@@ -7444,14 +7399,10 @@ public class OpticalFlow {
double [][] ignp = null; double [][] ignp = null;
if (gnp != null) { if (gnp != null) {
ignp = ErsCorrection.invertXYZATR(gnp); ignp = ErsCorrection.invertXYZATR(gnp);
// stereo_offset = new double [] {-ground_normal_pose[0][0],-ground_normal_pose[0][1],-ground_normal_pose[0][2]};
stereo_offset = gnp[0]; // stereo_pose[0];
stereo_atr = gnp[1]; // stereo_pose[1];
// stereo_atr = new double [] {-ignp[1][0],-ignp[1][1],-ignp[1][2]}; // stereo_pose[1];
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("Using airplane mode terrane as a single horizontal plane"); System.out.println("Using airplane mode terrane as a single horizontal plane");
System.out.println("stereo_offset = ["+stereo_offset[0]+", "+stereo_offset[1]+", "+stereo_offset[2]+"]"); System.out.println("stereo_offset = ["+gnp[0][0]+", "+gnp[0][1]+", "+gnp[0][2]+"]");
System.out.println("stereo_atr = ["+stereo_atr[0]+ ", "+stereo_atr[1]+ ", "+stereo_atr[2]+ "]"); System.out.println("stereo_atr = ["+gnp[1][0]+", "+gnp[1][1]+", "+gnp[1][2]+ "]");
// TODO add provisions to the non-flat terrain -modify terrain slice, keeping ground_normal_pose as a horizontal plane. // TODO add provisions to the non-flat terrain -modify terrain slice, keeping ground_normal_pose as a horizontal plane.
System.out.println("ignp[0] = ["+ignp[0][0]+", "+ignp[0][1]+", "+ignp[0][2]+"]"); System.out.println("ignp[0] = ["+ignp[0][0]+", "+ignp[0][1]+", "+ignp[0][2]+"]");
System.out.println("ignp[1] = ["+ignp[1][0]+", "+ignp[1][1]+", "+ignp[1][2]+"]"); System.out.println("ignp[1] = ["+ignp[1][0]+", "+ignp[1][1]+", "+ignp[1][2]+"]");
...@@ -7463,10 +7414,10 @@ public class OpticalFlow { ...@@ -7463,10 +7414,10 @@ public class OpticalFlow {
} else { } else {
double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN]; double [] terrain_disparity = combo_dsn_final[COMBO_DSN_INDX_TERRAIN];
String scenes_suffix = master_CLT.getImageName()+"-ITERRAIN4"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN"; String scenes_suffix = master_CLT.getImageName()+"-TERRAIN"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain = renderSceneSequence( ImagePlus imp_terrain = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
true, // master_CLT.hasCenterClt(), // boolean mode_cuas, false, // master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono, false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average, clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice, null, // float [] average_slice,
...@@ -7475,9 +7426,10 @@ public class OpticalFlow { ...@@ -7475,9 +7426,10 @@ public class OpticalFlow {
null, // Rectangle fov_tiles, null, // Rectangle fov_tiles,
1, // int mode3d, 1, // int mode3d,
false, // boolean toRGB, false, // boolean toRGB,
ignp[0], // ZERO3, // stereo_offset, // ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z} ignp, // double [][] ground_xyzatr,
gnp[1], // null, // stereo_atr, // null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas) ZERO3, //ignp[0], // ZERO3, // stereo_offset, // ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
// ignp, // new double [][] {stereo_offset,stereo_atr}, // null, //gnp[1], // ZERO3, //stereo_atr, // null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
null, // ignp, // new double [][] {stereo_offset,stereo_atr}, //
1, // int sensor_mask, 1, // int sensor_mask,
scenes_suffix, // String suffix, scenes_suffix, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity, terrain_disparity, // selected_disparity, // double [] ref_disparity,
...@@ -7488,37 +7440,11 @@ public class OpticalFlow { ...@@ -7488,37 +7440,11 @@ public class OpticalFlow {
master_CLT.saveImagePlusInModelDirectory( master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain); // imp_scenes); // ImagePlus imp) imp_terrain); // imp_scenes); // ImagePlus imp)
String scenes_suffix_flat = master_CLT.getImageName()+"-TERRAIN-FLAT"; // quadCLTs[quadCLTs.length-1][quadCLTs.length-1].getImageName()+"-TERRAIN";
ImagePlus imp_terrain_flat = renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
master_CLT.hasCenterClt(), // boolean mode_cuas,
false, // boolean um_mono,
clt_parameters.imp.add_average, // boolean insert_average,
null, // float [] average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // Rectangle fov_tiles,
1, // int mode3d,
false, // boolean toRGB,
ZERO3, // stereo_offset, // ZERO3, // stereo_offset, // ZERO3, // double [] stereo_offset, // offset reference camera {x,y,z}
null, // stereo_atr, // null, // stereo_atr, // null, // double [] stereo_atr, // offset reference orientation (cuas)
// gnp, // new double [][] {stereo_offset,stereo_atr}, //
1, // int sensor_mask,
scenes_suffix_flat, // String suffix,
terrain_disparity, // selected_disparity, // double [] ref_disparity,
quadCLTs, // QuadCLT [] quadCLTs,
master_CLT, // ref_index, // int ref_index,
threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
master_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_terrain_flat); // imp_scenes); // ImagePlus imp)
} }
} }
if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) { if ((combo_dsn_final.length <= COMBO_DSN_INDX_TERRAIN) || (combo_dsn_final[COMBO_DSN_INDX_TERRAIN] == null)) {
System.out.println ("No terrain data available"); System.out.println ("No terrain data available");
} else { } else {
...@@ -9023,6 +8949,7 @@ public class OpticalFlow { ...@@ -9023,6 +8949,7 @@ public class OpticalFlow {
fov_tiles, // Rectangle fov_tiles, fov_tiles, // Rectangle fov_tiles,
mode3d, // int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG mode3d, // int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG
toRGB, // boolean toRGB, toRGB, // boolean toRGB,
null, // double [][] ground_xyzatr,
stereo_xyz, // double [] stereo_xyz, // offset reference camera {x,y,z} stereo_xyz, // double [] stereo_xyz, // offset reference camera {x,y,z}
stereo_atr_in, // double [] stereo_atr_in, // offset reference orientation (cuas) stereo_atr_in, // double [] stereo_atr_in, // offset reference orientation (cuas)
null, // double [][] post_rotate, null, // double [][] post_rotate,
...@@ -9045,12 +8972,13 @@ public class OpticalFlow { ...@@ -9045,12 +8972,13 @@ public class OpticalFlow {
Rectangle fov_tiles, Rectangle fov_tiles,
int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG int mode3d, // for older compatibility mode3d = -1 for RAW, 0 - INF, 1 - FG, 2 BG
boolean toRGB, boolean toRGB,
double [][] ground_xyzatr,
double [] stereo_xyz, // offset reference camera {x,y,z} double [] stereo_xyz, // offset reference camera {x,y,z}
double [] stereo_atr_in, // offset reference orientation (cuas) double [] stereo_atr_in, // offset reference orientation (cuas)
double [][] post_rotate, double [][] post_rotate,
int sensor_mask, int sensor_mask,
String suffix_in, String suffix_in,
double [] ref_disparity, double [] ref_disparity, // may be ground disparity
QuadCLT [] quadCLTs, QuadCLT [] quadCLTs,
QuadCLT ref_scene, // int ref_index, QuadCLT ref_scene, // int ref_index,
int threadsMax, int threadsMax,
...@@ -9106,7 +9034,85 @@ public class OpticalFlow { ...@@ -9106,7 +9034,85 @@ public class OpticalFlow {
int dbg_scene = -95; int dbg_scene = -95;
double [][] ref_pXpYD; double [][] ref_pXpYD;
double [][] ref_pXpYD_or_null = null; // debugging cuas mode keeping old double [][] ref_pXpYD_or_null = null; // debugging cuas mode keeping old
if (mode_cuas) { // && (dbg_scene > 0)) { if (ground_xyzatr != null) { // extract to method
final double [] ground_disparity = ref_disparity; // input
final int tilesX=ref_scene.getTilesX();
final int tilesY=ref_scene.getTilesY();
final int transform_size = ref_scene.getTileSize();
final int tiles = tilesX * tilesY;
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final double [][] fgnd_pXpYD = new double [tiles][];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nTile = ai.getAndIncrement(); nTile < tiles; nTile = ai.getAndIncrement()) if (!Double.isNaN(ground_disparity[nTile])){
double disparity = ground_disparity[nTile];
int tileY = nTile / tilesX;
int tileX = nTile % tilesX;
double centerX = (tileX + 0.5) * transform_size; // - shiftX;
double centerY = (tileY + 0.5) * transform_size; // - shiftY;
if (disparity < 0) {
disparity = 1.0* disparity; // 0.0;
}
fgnd_pXpYD[nTile] = new double[] {centerX, centerY, disparity};
// fgnd_pXpYD[nTile] = new double[] {transform_size*tilesX-centerX, transform_size*tilesY-centerY, disparity};
}
}
};
}
ImageDtt.startAndJoin(threads);
ref_pXpYD = OpticalFlow.transformToScenePxPyD(
fgnd_pXpYD, // final double [][] reference_pXpYD,// invalid tiles - NaN in disparity. Should be no nulls, no NaN disparity
ground_xyzatr[0], // final double [] scene_xyz, // camera center in world (reference) coordinates
ground_xyzatr[1], // final double [] scene_atr, // camera orientation relative to world (reference) frame
ref_scene, // final QuadCLT reference_QuadClt) //
null); // final QuadCLT scene_QuadClt) //
/*
double [] disp_dbg = new double[tiles];
Arrays.fill(disp_dbg, Double.NaN);
for (int i = 0; i < tiles; i++) {
if (ref_pXpYD[i] != null) {
disp_dbg[i] = ref_pXpYD[i][2];
}
}
for (int i = 0; i < tiles; i++) {
int i1 = tiles-i-1;
if (ref_pXpYD[i] != null) {
if (ref_pXpYD[i1] != null) {
ref_pXpYD[i][2]=disp_dbg[i1];
} else {
ref_pXpYD[i] = null;
}
}
}
*/
ref_pXpYD_or_null = ref_pXpYD;
ref_scene.getErsCorrection().setupERS();
System.out.println("Calculated reference ref_pXpYD from virtual_PxPyD");
boolean debug_virtual_PxPyD = (debugLevel > 1000);
if (debug_virtual_PxPyD) {
String [] dbg_titles = {"gnd-pX", "gnd-pY", "gnd-D","ref-pX", "ref-pY", "ref-D"};
double [][] dbg_pXpYD = new double[dbg_titles.length][ref_pXpYD.length];
for (int i = 0; i < dbg_pXpYD.length; i++) Arrays.fill(dbg_pXpYD[i], Double.NaN);
for (int i = 0; i < fgnd_pXpYD.length; i++) if (fgnd_pXpYD[i] != null) {
for (int j = 0; j < fgnd_pXpYD[i].length; j++) dbg_pXpYD[j][i] = fgnd_pXpYD[i][j];
}
for (int i = 0; i < ref_pXpYD.length; i++) if (ref_pXpYD[i] != null) {
for (int j = 0; j < ref_pXpYD[i].length; j++) dbg_pXpYD[j+3][i] = ref_pXpYD[i][j];
}
ImagePlus imp_virtual_PxPyD= ShowDoubleFloatArrays.makeArrays(
dbg_pXpYD,
ref_scene.getTilesX(),
ref_scene.getTilesY(),
ref_scene.getImageName()+"-ref_from_virtual_PxPyD",
dbg_titles);
ref_scene.saveImagePlusInModelDirectory(
null, // String suffix,
imp_virtual_PxPyD); // ImagePlus imp)
}
} else if (mode_cuas) { // && (dbg_scene > 0)) {
int around = 2; int around = 2;
double around_sigma = 4.0; double around_sigma = 4.0;
int num_virtual_refines = 2; int num_virtual_refines = 2;
......
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