Skip to content

I
imagej-elphel
Commit 22cc393a authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Implemented iterative optical flow calculation

parent 78002a6b
Hide whitespace changes
Inline Side-by-side
Showing with 768 additions and 379 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 22cc393a
...@@ -98,7 +98,206 @@ public class OpticalFlow { ...@@ -98,7 +98,206 @@ public class OpticalFlow {
System.out.println("fillTilesNans() DONE."); System.out.println("fillTilesNans() DONE.");
} }
public double [][] correlate2DIterate( // returns optical flow and confidence
final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
// for prepareSceneTiles()
final double [] scene_xyz, // camera center in world coordinates
final double [] scene_atr, // camera orientation relative to world frame
final QuadCLT scene_QuadClt,
final QuadCLT reference_QuadClt,
final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
// flowXY should be initialized to all pairs of zeros (or deliberate pixel offset pairs if initial error is too high, will be modified with each iteration
final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions // initialize to [reference_tiles.length][2]
final double tolerance_absolute, // absolute disparity half-range in each tile
final double tolerance_relative, // relative disparity half-range in each tile
final double occupancy, // fraction of remaining tiles (<1.0)
final int num_passes,
final double max_change,
// for correlate2DSceneToReference ()
final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
final double corr_sigma,
final double fat_zero,
final boolean late_normalize,
// for correlation2DToVectors_CM()
final int iradius, // half-size of the square to process
final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
final int refine_num, // number of iterations to apply weights around new center
final int num_run_all, // run all tiles for few iterations before filtering
final int max_tries,
// for recalculateFlowXY()
final double magic_scale, // 0.85 for CM
final double min_change,
final int debug_level)
{
final TileProcessor tp = reference_QuadClt.getTileProcessor();
final int transform_size = tp.getTileSize();
final double [][] flowXY_frac = new double [reference_tiles.length][]; // Will contain fractional X/Y shift for CLT
double [][] flowXY_run = flowXY; // only non-nulls for the tiles to correlate
final double [] abs_change = new double [reference_tiles.length]; // updated
Arrays.fill(abs_change, Double.NaN);
for (int ntry = 0; ntry < max_tries; ntry++) {
double [][][] scene_tiles = prepareSceneTiles(// to match to reference
// null for {scene,reference}{xyz,atr} uses instances globals
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadClt, // final QuadCLT scene_QuadClt,
reference_QuadClt, // final QuadCLT reference_QuadClt,
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
flowXY_run, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions
flowXY_frac, // final double [][] flowXY_frac, // should be initialized as [number of macro tiles][] - returns fractional shifts [-0.5, 0.5)
tolerance_absolute, // final double tolerance_absolute, // absolute disparity half-range in each tile
tolerance_absolute, // final double tolerance_relative, // relative disparity half-range in each tile
tolerance_relative, // final double occupancy, // fraction of remaining tiles (<1.0)
num_passes, // final int num_passes,
max_change, // final double max_change,
-1); //-1); // 1); // 2); // final int debug_level)
double [][] corr2dscene_ref = correlate2DSceneToReference(// to match to reference
imgdtt_params, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
scene_QuadClt, // final QuadCLT scene_QuadClt,
reference_QuadClt, // final QuadCLT reference_QuadClt,
scene_tiles, // final double [][][] scene_tiles, // prepared with prepareSceneTiles()
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine?
flowXY_frac, // final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero,
late_normalize, // final boolean late_normalize,
false, // final boolean combine_empty_only, // only use neighbor correlations for empty tiles (false - any)
0.0, // final double combine_dradius
0.0, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
0.0, // final double tolerance_relative, // relative disparity half-range to consolidate tiles
-1); // 1); // final int debug_level)
double [][] vectorsXYS = correlation2DToVectors_CM(
corr2dscene_ref, // final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls)
transform_size, // final int transform_size,
iradius, // final int iradius, // half-size of the square to process
dradius, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
refine_num, // final int refine_num, // number of iterations to apply weights around new center
-1); //final int debug_level)
flowXY_run = recalculateFlowXY(
flowXY, // final double [][] flowXY, // will update
vectorsXYS, // final double [][] corr_vectorsXY,
abs_change, // final double [] abs_change, // updated
magic_scale/transform_size, // final double magic_scale, // 0.85 for CM
((ntry < num_run_all)? 0.0: min_change), // final double min_change,
1); // final int debug_level);
if (flowXY_run == null) { // nothing to do left
break;
}
}
return flowXY; // it is also in input arguments
}
double [][] recalculateFlowXY(
final double [][] currentFlowXY,
final double [][] corr_vectorsXY,
final double magic_scale) // 0.85 for CM
{
final Thread[] threads = ImageDtt.newThreadArray(threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final double [][] flowXY = new double [currentFlowXY.length][];
final int dbg_mtile = 620; // 453; // 500;
final double rmagic_scale = 1.0/magic_scale;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < currentFlowXY.length; iMTile = ai.getAndIncrement())
if ((currentFlowXY[iMTile] != null) && (corr_vectorsXY[iMTile] != null)){
if (iMTile == dbg_mtile) {
System.out.println("iMTile = "+iMTile);
}
flowXY[iMTile]= new double[] {
currentFlowXY[iMTile][0] + rmagic_scale * corr_vectorsXY[iMTile][0],
currentFlowXY[iMTile][1] + rmagic_scale * corr_vectorsXY[iMTile][1]};
}
}
};
}
ImageDtt.startAndJoin(threads);
return flowXY;
}
/**
* Recalculate optical flow vector (in image pixels)
* @param flowXY current per-tile vectors (null for undefined), updated
* @param corr_vectorsXY correction vector from correlation to apply
* @param abs_change absolute value of last coordinate change for each tile
* @param magic_scale divide correlation vector (typically 0.85/8 for CM argmax)
* @param min_change minimal vector coordinate difference to repeat correlations
* @param debug_level if > 0; print number of tiles to correlate
* @return flowXY vectors only for tiles to be updated or null if no tiles left
*/
double [][] recalculateFlowXY(
final double [][] flowXY, // will update
final double [][] corr_vectorsXY,
final double [] abs_change, // updated
final double magic_scale, // 0.85 for CM
final double min_change,
final int debug_level)
{
final Thread[] threads = ImageDtt.newThreadArray(threadsMax);
final AtomicInteger ai = new AtomicInteger(0);
final double [][] flowXY_task = new double [flowXY.length][];
final int dbg_mtile = 620; // 453; // 500;
final double rmagic_scale = 1.0/magic_scale;
final AtomicInteger aupdate = new AtomicInteger(0); //number of tiles to recalculate
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int iMTile = ai.getAndIncrement(); iMTile < flowXY.length; iMTile = ai.getAndIncrement()) {
if (iMTile == dbg_mtile) {
System.out.println("iMTile = "+iMTile);
}
if (flowXY[iMTile] != null){
if (corr_vectorsXY[iMTile] == null) {
if (min_change <= 0.0) {
flowXY_task[iMTile] = flowXY[iMTile];
abs_change[iMTile] = Double.NaN;
aupdate.getAndIncrement();
}
} else { // if (corr_vectorsXY[iMTile] == null)
double dx = rmagic_scale * corr_vectorsXY[iMTile][0];
double dy = rmagic_scale * corr_vectorsXY[iMTile][1];
// apply correction in any case
flowXY[iMTile][0] += dx;
flowXY[iMTile][1] += dy;
double new_diff = Math.sqrt(dx*dx + dy*dy);
double last_change = abs_change[iMTile]; // may be NaN;
abs_change[iMTile] = new_diff;
if (!(new_diff > last_change) && (new_diff > min_change)) { // not worse (any value are not worse than NaN) and larger than threshold
flowXY_task[iMTile] = flowXY[iMTile]; // set to measure
aupdate.getAndIncrement();
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
if (debug_level > 0) {
System.out.println("recalculateFlowXY(): tiles to correlate: "+aupdate.get());
}
if (aupdate.get() > 0) {
return flowXY_task;
} else {
return null; // nothing to measure left
}
}
public double [][] correlation2DToVectors_CM( public double [][] correlation2DToVectors_CM(
final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls) final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls)
final int transform_size, final int transform_size,
...@@ -196,13 +395,13 @@ public class OpticalFlow { ...@@ -196,13 +395,13 @@ public class OpticalFlow {
final QuadCLT reference_QuadClt, final QuadCLT reference_QuadClt,
final double [][][] scene_tiles, // prepared with prepareSceneTiles() final double [][][] scene_tiles, // prepared with prepareSceneTiles()
final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine? final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine?
// final double [][] flowXY, // per macro tile {mismatch in IMAGE_PIXELS in X and Y directions (pre-shift)
// final int combine
final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations
final double [] chn_weights, // absolute, starting from strength (strength,r,b,g) final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
final double corr_sigma, final double corr_sigma,
final double fat_zero, final double fat_zero,
final boolean late_normalize, final boolean late_normalize,
final boolean combine_empty_only, // only use neighbor correlations for empty corr tiles (false - any)
// reference tile should still be defined
final double combine_dradius, // 1 - 3x3, 2 - 5x5 final double combine_dradius, // 1 - 3x3, 2 - 5x5
final double tolerance_absolute, // absolute disparity half-range to consolidate tiles final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
final double tolerance_relative, // relative disparity half-range to consolidate tiles final double tolerance_relative, // relative disparity half-range to consolidate tiles
...@@ -369,22 +568,15 @@ public class OpticalFlow { ...@@ -369,22 +568,15 @@ public class OpticalFlow {
2.0, // double wndx_scale, // (wndy scale is always 1.0) 2.0, // double wndx_scale, // (wndy scale is always 1.0)
false, // boolean monochrome, false, // boolean monochrome,
false); // boolean debug) false); // boolean debug)
// reference tile should not be null, scene = may be
for (int iMTile = ai.getAndIncrement(); iMTile < macroTiles; iMTile = ai.getAndIncrement()) { for (int iMTile = ai.getAndIncrement(); iMTile < macroTiles; iMTile = ai.getAndIncrement()) if (reference_tiles[iMTile] != null){
// if (((scene_tiles[iMTile] != null) && (reference_tiles[iMTile] != null)) || (combine_radius > 0)) { // if (((scene_tiles[iMTile] != null) && (reference_tiles[iMTile] != null)) || (combine_radius > 0)) {
if ((scene_tiles[iMTile] != null) && (reference_tiles[iMTile] != null)) { // if ((scene_tiles[iMTile] != null) && (reference_tiles[iMTile] != null)) {
if (true) { // !combine_empty_only || (corr_tiles_TD[iMTile] == null)) {
if (iMTile == dbg_mtile) { if (iMTile == dbg_mtile) {
System.out.println("iMTile = "+iMTile); System.out.println("iMTile = "+iMTile);
} }
/* if ((combine_radius > 0) && (!combine_empty_only || (corr_tiles_TD[iMTile] == null))) { //
if ((scene_tiles[iMTile] != null) && (reference_tiles[iMTile] != null)) {
for (int i = 0; i< 4; i++) {
System.arraycopy(corr_tiles_TD[iMTile][i], 0, corr_tile_2D[i], 0, tile_length);
}
num_tiles
}
*/
if (combine_radius > 0) {
for (int q = 0; q< 4; q++) { for (int q = 0; q< 4; q++) {
Arrays.fill(corr_tile_2D[q], 0.0); Arrays.fill(corr_tile_2D[q], 0.0);
} }
...@@ -488,364 +680,346 @@ public class OpticalFlow { ...@@ -488,364 +680,346 @@ public class OpticalFlow {
double wdiag = 0.25 *diagonal_weight / (diagonal_weight + 1.0); double wdiag = 0.25 *diagonal_weight / (diagonal_weight + 1.0);
double wortho = 0.25 / (diagonal_weight + 1.0); double wortho = 0.25 / (diagonal_weight + 1.0);
final double [] neibw = {wortho, wdiag, wortho, wdiag, wortho, wdiag, wortho, wdiag}; final double [] neibw = {wortho, wdiag, wortho, wdiag, wortho, wdiag, wortho, wdiag};
final int dbg_mtile = 453; // 500; final int dbg_mtile = 54; // 453; // 500;
for (int ithread = 0; ithread < threads.length; ithread++) { for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() { threads[ithread] = new Thread() {
public void run() { public void run() {
// double [][] pXpYD = new double [fullTileLen][]; // double [][] pXpYD = new double [fullTileLen][];
double [][] tXtYD = new double [fullTileLen][]; // measured in tiles, not pixels (disparity - still pixels) double [][] tXtYD = new double [fullTileLen][]; // measured in tiles, not pixels (disparity - still pixels)
for (int iMTile = ai.getAndIncrement(); iMTile < reference_tiles.length; iMTile = ai.getAndIncrement()) if (reference_tiles[iMTile] != null){ for (int iMTile = ai.getAndIncrement(); iMTile < reference_tiles.length; iMTile = ai.getAndIncrement())
if (iMTile == dbg_mtile) { if ((reference_tiles[iMTile] != null) && (flowXY[iMTile] != null)){
System.out.println("iMTile = "+iMTile); if (iMTile == dbg_mtile) {
} System.out.println("iMTile = "+iMTile);
int mtileY = iMTile / macroTilesX; }
int mtileX = iMTile % macroTilesX; int mtileY = iMTile / macroTilesX;
int tY0 = mtileY * transform_size + macroY0 -transform_size/2 - margin; int mtileX = iMTile % macroTilesX;
int tX0 = mtileX * transform_size + macroX0 -transform_size/2 - margin; int tY0 = mtileY * transform_size + macroY0 -transform_size/2 - margin;
// Arrays.fill(pXpYD, null); int tX0 = mtileX * transform_size + macroX0 -transform_size/2 - margin;
Arrays.fill(tXtYD, null); // Arrays.fill(pXpYD, null);
for (int iY = 0; iY < fullTileSize; iY++) { Arrays.fill(tXtYD, null);
int tileY = tY0 + iY; for (int iY = 0; iY < fullTileSize; iY++) {
if ((tileY >= 0) && (tileY < tilesY)) { int tileY = tY0 + iY;
for (int iX = 0; iX < fullTileSize; iX++) { if ((tileY >= 0) && (tileY < tilesY)) {
int tileX = tX0 + iX; for (int iX = 0; iX < fullTileSize; iX++) {
if ((tileX >= 0) && (tileX < tilesX)) { int tileX = tX0 + iX;
// int nTile = tileX + tileY * tilesX; if ((tileX >= 0) && (tileX < tilesX)) {
int iTile = iX + iY * fullTileSize; // int nTile = tileX + tileY * tilesX;
int iTile = iX + iY * fullTileSize;
double disparity = reference_tiles[iMTile][QuadCLT.DSRBG_DISPARITY][iTile];
if (disparity < 0) { double disparity = reference_tiles[iMTile][QuadCLT.DSRBG_DISPARITY][iTile];
disparity = 0.0; // is it needed or should it work with negative too? if (disparity < 0) {
} disparity = 0.0; // is it needed or should it work with negative too?
double centerX = tileX * transform_size + transform_size/2; // - shiftX; }
double centerY = tileY * transform_size + transform_size/2; // - shiftY; double centerX = tileX * transform_size + transform_size/2; // - shiftX;
if (!Double.isNaN(disparity)) { double centerY = tileY * transform_size + transform_size/2; // - shiftY;
double [] xyd = ers_reference.getImageCoordinatesERS( if (!Double.isNaN(disparity)) {
scene_QuadClt, // QuadCLT cameraQuadCLT, // camera station that got image to be to be matched double [] xyd = ers_reference.getImageCoordinatesERS(
centerX, // double px, // pixel coordinate X in this camera view scene_QuadClt, // QuadCLT cameraQuadCLT, // camera station that got image to be to be matched
centerY, //double py, // pixel coordinate Y in this camera view centerX, // double px, // pixel coordinate X in this camera view
disparity, // double disparity, // this view disparity centerY, //double py, // pixel coordinate Y in this camera view
true, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear disparity, // double disparity, // this view disparity
ZERO3, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3) true, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
ZERO3, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3) ZERO3, // double [] reference_xyz, // this view position in world coordinates (typically ZERO3)
true, // boolean distortedCamera, // camera view is distorted (false - rectilinear) ZERO3, // double [] reference_atr, // this view orientation relative to world frame (typically ZERO3)
scene_xyz, // double [] camera_xyz, // camera center in world coordinates true, // boolean distortedCamera, // camera view is distorted (false - rectilinear)
scene_atr, // double [] camera_atr, // camera orientation relative to world frame scene_xyz, // double [] camera_xyz, // camera center in world coordinates
LINE_ERR); // double LINE_ERR) // threshold error in scan lines (1.0) scene_atr, // double [] camera_atr, // camera orientation relative to world frame
if (xyd != null) { LINE_ERR); // double LINE_ERR) // threshold error in scan lines (1.0)
tXtYD[iTile] = new double [] { if (xyd != null) {
((xyd[0] + flowXY[iMTile][0])/transform_size), tXtYD[iTile] = new double [] {
((xyd[1] + flowXY[iMTile][0])/transform_size), ((xyd[0] + flowXY[iMTile][0])/transform_size),
xyd[2]}; ((xyd[1] + flowXY[iMTile][1])/transform_size),
xyd[2]};
} else {
tXtYD[iTile] = null;
}
} else { } else {
tXtYD[iTile] = null; tXtYD[iTile] = null;
} }
} else {
tXtYD[iTile] = null;
} }
} }
} }
} }
} if ((debug_level>1) && (iMTile == dbg_mtile)) {
if ((debug_level>1) && (iMTile == dbg_mtile)) { String [] dbg_titles= {"tX","tY","disp"};
String [] dbg_titles= {"tX","tY","disp"}; String dbg_title= "tXtYD-MX"+mtileX+"_MY"+mtileY;
String dbg_title= "tXtYD-MX"+mtileX+"_MY"+mtileY; double [][] dbg_img = new double [3][fullTileLen];
double [][] dbg_img = new double [3][fullTileLen]; for (int nt =0; nt < fullTileLen; nt++) {
for (int nt =0; nt < fullTileLen; nt++) { if(tXtYD[nt] != null) {
if(tXtYD[nt] != null) { for (int i=0; i < dbg_img.length; i++) dbg_img[i][nt] = tXtYD[nt][i];
for (int i=0; i < dbg_img.length; i++) dbg_img[i][nt] = tXtYD[nt][i]; } else {
} else { for (int i=0; i < dbg_img.length; i++) dbg_img[i][nt] = Double.NaN;
for (int i=0; i < dbg_img.length; i++) dbg_img[i][nt] = Double.NaN; }
} }
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
fullTileSize,
fullTileSize,
true,
dbg_title,
dbg_titles);
} }
(new ShowDoubleFloatArrays()).showArrays(
dbg_img, // Find best fractional pixel offset
fullTileSize, double [] hist_fx = new double [hist_len];
fullTileSize, double [] hist_fy = new double [hist_len];
true, for (int iY = 0; iY < fullTileSize; iY++) if (hist_weights[iY] > 0.0){
dbg_title, for (int iX = 0; iX < fullTileSize; iX++) if (hist_weights[iX] > 0.0){
dbg_titles); int iTile = iX + iY * fullTileSize;
} if (tXtYD[iTile] != null) {
int hidx = (int) Math.floor((tXtYD[iTile][0] - Math.floor(tXtYD[iTile][0])) * hist_len);
// Find best fractional pixel offset if (hidx < 0) hidx = 0;
double [] hist_fx = new double [hist_len]; else if (hidx >= hist_len) hidx = hist_len - 1;
double [] hist_fy = new double [hist_len]; hist_fx[hidx] += hist_weights[iX];
for (int iY = 0; iY < fullTileSize; iY++) if (hist_weights[iY] > 0.0){ int hidy = (int) Math.floor((tXtYD[iTile][1] - Math.floor(tXtYD[iTile][1])) * hist_len);
for (int iX = 0; iX < fullTileSize; iX++) if (hist_weights[iX] > 0.0){ if (hidy < 0) hidy = 0;
int iTile = iX + iY * fullTileSize; else if (hidy >= hist_len) hidy = hist_len - 1;
if (tXtYD[iTile] != null) { hist_fy[hidy] += hist_weights[iY];
int hidx = (int) Math.floor((tXtYD[iTile][0] - Math.floor(tXtYD[iTile][0])) * hist_len); }
if (hidx < 0) hidx = 0;
else if (hidx >= hist_len) hidx = hist_len - 1;
hist_fx[hidx] += hist_weights[iX];
int hidy = (int) Math.floor((tXtYD[iTile][1] - Math.floor(tXtYD[iTile][1])) * hist_len);
if (hidy < 0) hidy = 0;
else if (hidy >= hist_len) hidy = hist_len - 1;
hist_fy[hidy] += hist_weights[iY];
} }
} }
} int hist_fx_mx = 0;
int hist_fx_mx = 0; int hist_fy_mx = 0;
int hist_fy_mx = 0; for (int i = 1; i < hist_len; i++) {
for (int i = 1; i < hist_len; i++) { if (hist_fx[i] > hist_fx[hist_fx_mx]) hist_fx_mx = i;
if (hist_fx[i] > hist_fx[hist_fx_mx]) hist_fx_mx = i; if (hist_fy[i] > hist_fy[hist_fy_mx]) hist_fy_mx = i;
if (hist_fy[i] > hist_fy[hist_fy_mx]) hist_fy_mx = i; }
} double offsX = (0.5 + hist_fx_mx) / hist_len;
double offsX = (0.5 + hist_fx_mx) / hist_len; double offsY = (0.5 + hist_fy_mx) / hist_len;
double offsY = (0.5 + hist_fy_mx) / hist_len; double swx = 0.0, swy = 0.0, sx = 0.0, sy = 0.0;
double swx = 0.0, swy = 0.0, sx = 0.0, sy = 0.0; for (int iY = 0; iY < fullTileSize; iY++) {
for (int iY = 0; iY < fullTileSize; iY++) { for (int iX = 0; iX < fullTileSize; iX++) {
for (int iX = 0; iX < fullTileSize; iX++) { int iTile = iX + iY * fullTileSize;
int iTile = iX + iY * fullTileSize; if (tXtYD[iTile] != null) {
if (tXtYD[iTile] != null) { double wx = hist_weights[iX];
double wx = hist_weights[iX]; double wy = hist_weights[iX];
double wy = hist_weights[iX]; double dx = tXtYD[iTile][0] - offsX;
double dx = tXtYD[iTile][0] - offsX; double dy = tXtYD[iTile][1] - offsY;
double dy = tXtYD[iTile][1] - offsY; dx = dx - Math.round(dx);
dx = dx - Math.round(dx); dy = dy - Math.round(dy);
dy = dy - Math.round(dy); swx += wx;
swx += wx; swy += wy;
swy += wy; sx += wx * dx;
sx += wx * dx; sy += wy * dy;
sy += wy * dy; }
} }
} }
} offsX += sx/swx;
offsX += sx/swx; offsY += sy/swy;
offsY += sy/swy; // use offsX, offsY as fractional shift and for data interpolation
// use offsX, offsY as fractional shift and for data interpolation if (offsX >= .5) offsX -= 1.0;
if (offsX >= .5) offsX -= 1.0; if (offsY >= .5) offsY -= 1.0;
if (offsY >= .5) offsY -= 1.0; flowXY_frac[iMTile] = new double [] {-offsX, -offsY};
flowXY_frac[iMTile] = new double [] {-offsX, -offsY}; double min_tX = Double.NaN, max_tX = Double.NaN, min_tY = Double.NaN, max_tY = Double.NaN;
double min_tX = Double.NaN, max_tX = Double.NaN, min_tY = Double.NaN, max_tY = Double.NaN; for (int iY = 0; iY < fullTileSize; iY++) {
for (int iY = 0; iY < fullTileSize; iY++) { for (int iX = 0; iX < fullTileSize; iX++) {
for (int iX = 0; iX < fullTileSize; iX++) { int iTile = iX + iY * fullTileSize;
int iTile = iX + iY * fullTileSize; if (tXtYD[iTile] != null) {
if (tXtYD[iTile] != null) { tXtYD[iTile][0]-=offsX;
tXtYD[iTile][0]-=offsX; tXtYD[iTile][1]-=offsY;
tXtYD[iTile][1]-=offsY; if (Double.isNaN(min_tX)) {
if (Double.isNaN(min_tX)) { min_tX =tXtYD[iTile][0];
min_tX =tXtYD[iTile][0]; min_tY =tXtYD[iTile][1];
min_tY =tXtYD[iTile][1]; max_tX = min_tX;
max_tX = min_tX; max_tY = min_tY;
max_tY = min_tY; }
if (min_tX > tXtYD[iTile][0]) min_tX = tXtYD[iTile][0];
if (min_tY > tXtYD[iTile][1]) min_tY = tXtYD[iTile][1];
if (max_tX < tXtYD[iTile][0]) max_tX = tXtYD[iTile][0];
if (max_tY < tXtYD[iTile][1]) max_tY = tXtYD[iTile][1];
} }
if (min_tX > tXtYD[iTile][0]) min_tX = tXtYD[iTile][0];
if (min_tY > tXtYD[iTile][1]) min_tY = tXtYD[iTile][1];
if (max_tX < tXtYD[iTile][0]) max_tX = tXtYD[iTile][0];
if (max_tY < tXtYD[iTile][1]) max_tY = tXtYD[iTile][1];
} }
} }
} int imin_tX = (int) Math.floor(min_tX);
int imin_tX = (int) Math.floor(min_tX); int imin_tY = (int) Math.floor(min_tY);
int imin_tY = (int) Math.floor(min_tY); int imax_tX = (int) Math.ceil (max_tX);
int imax_tX = (int) Math.ceil (max_tX); int imax_tY = (int) Math.ceil (max_tY);
int imax_tY = (int) Math.ceil (max_tY); // See if at least some of fits into the frame
// See if at least some of fits into the frame if ((imin_tX >= tilesX) || (imin_tY >= tilesY) || (imax_tX < 0) || (imax_tX < 0)) {
if ((imin_tX >= tilesX) || (imin_tY >= tilesY) || (imax_tX < 0) || (imax_tX < 0)) { continue; // no overlap at all
continue; // no overlap at all }
} int iwidth = imax_tX - imin_tX + 1;
int iwidth = imax_tX - imin_tX + 1; int iheight = imax_tY - imin_tY + 1;
int iheight = imax_tY - imin_tY + 1; double [][] scene_slices = new double [dsrbg_scene.length][iwidth*iheight];
double [][] scene_slices = new double [dsrbg_scene.length][iwidth*iheight]; for (int iY = 0; iY < iheight; iY++) {
for (int iY = 0; iY < iheight; iY++) { int tY = imin_tY + iY;
int tY = imin_tY + iY; if ((tY >= 0) && (tY < tilesY)) {
if ((tY >= 0) && (tY < tilesY)) { for (int iX = 0; iX < iwidth; iX++) {
for (int iX = 0; iX < iwidth; iX++) { int tX = imin_tX + iX;
int tX = imin_tX + iX; if ((tX >= 0) && (tX < tilesX)) {
if ((tX >= 0) && (tX < tilesX)) { int iTile = iX + iY * iwidth;
int iTile = iX + iY * iwidth; int tile = tX + tilesX * tY;
int tile = tX + tilesX * tY; if (dsrbg_scene[QuadCLT.DSRBG_STRENGTH][tile] > 0.0) {
if (dsrbg_scene[QuadCLT.DSRBG_STRENGTH][tile] > 0.0) { double d = dsrbg_scene[QuadCLT.DSRBG_DISPARITY][tile];
double d = dsrbg_scene[QuadCLT.DSRBG_DISPARITY][tile]; scene_slices[QuadCLT.DSRBG_DISPARITY][iTile] = d;
scene_slices[QuadCLT.DSRBG_DISPARITY][iTile] = d; if (!Double.isNaN(d)) {
if (!Double.isNaN(d)) { scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] = dsrbg_scene[QuadCLT.DSRBG_STRENGTH][tile];
scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] = dsrbg_scene[QuadCLT.DSRBG_STRENGTH][tile]; }
} }
} }
} }
} }
} }
} if ((debug_level>1) && (iMTile == dbg_mtile)) {
if ((debug_level>1) && (iMTile == dbg_mtile)) { String [] dbg_titles= {"d","s","r","b","g"};
String [] dbg_titles= {"d","s","r","b","g"}; String dbg_title= "before_disparity-MX"+mtileX+"_MY"+mtileY;
String dbg_title= "before_disparity-MX"+mtileX+"_MY"+mtileY; (new ShowDoubleFloatArrays()).showArrays(
(new ShowDoubleFloatArrays()).showArrays( scene_slices,
scene_slices, iwidth,
iwidth, iheight,
iheight, true,
true, dbg_title,
dbg_title, dbg_titles);
dbg_titles); }
} // filter by disparity - 1 tile around rounded
// filter by disparity - 1 tile around rounded final TileNeibs tn = new TileNeibs(iwidth, iheight);
final TileNeibs tn = new TileNeibs(iwidth, iheight); for (int iY = 0; iY < fullTileSize; iY++) {
for (int iY = 0; iY < fullTileSize; iY++) { for (int iX = 0; iX < fullTileSize; iX++) {
for (int iX = 0; iX < fullTileSize; iX++) { int iTile = iX + iY * fullTileSize;
int iTile = iX + iY * fullTileSize; if (tXtYD[iTile] != null) {
if (tXtYD[iTile] != null) { double disp_tolerance = tolerance_absolute + tXtYD[iTile][2] * tolerance_relative;
double disp_tolerance = tolerance_absolute + tXtYD[iTile][2] * tolerance_relative; double disp_min = tXtYD[iTile][2] - disp_tolerance;
double disp_min = tXtYD[iTile][2] - disp_tolerance; double disp_max = tXtYD[iTile][2] + disp_tolerance;
double disp_max = tXtYD[iTile][2] + disp_tolerance; int nt = tn.getIndex(
int nt = tn.getIndex( (int) Math.round(tXtYD[iTile][0]) - imin_tX,
(int) Math.round(tXtYD[iTile][0]) - imin_tX, (int) Math.round(tXtYD[iTile][1]) - imin_tY);
(int) Math.round(tXtYD[iTile][1]) - imin_tY); if (nt >= 0) { // should always be
if (nt >= 0) { // should always be for (int dir = 0; dir <9; dir++) {
for (int dir = 0; dir <9; dir++) { int nt1 = tn.getNeibIndex(nt, dir);
int nt1 = tn.getNeibIndex(nt, dir); if ((nt1 >= 0) && (scene_slices[QuadCLT.DSRBG_STRENGTH][nt1] > 0.0)) {
if ((nt1 >= 0) && (scene_slices[QuadCLT.DSRBG_STRENGTH][nt1] > 0.0)) { if ( (scene_slices[QuadCLT.DSRBG_DISPARITY][nt1] < disp_min) ||
if ( (scene_slices[QuadCLT.DSRBG_DISPARITY][nt1] < disp_min) || (scene_slices[QuadCLT.DSRBG_DISPARITY][nt1] > disp_max)) {
(scene_slices[QuadCLT.DSRBG_DISPARITY][nt1] > disp_max)) { scene_slices[QuadCLT.DSRBG_STRENGTH][nt1] = 0.0; // disable tile
scene_slices[QuadCLT.DSRBG_STRENGTH][nt1] = 0.0; // disable tile }
} }
} }
} }
} }
} }
} }
} if ((debug_level>1) && (iMTile == dbg_mtile)) {
if ((debug_level>1) && (iMTile == dbg_mtile)) { String [] dbg_titles= {"d","s","r","b","g"};
String [] dbg_titles= {"d","s","r","b","g"}; String dbg_title= "after_disparity-MX"+mtileX+"_MY"+mtileY;
String dbg_title= "after_disparity-MX"+mtileX+"_MY"+mtileY; (new ShowDoubleFloatArrays()).showArrays(
(new ShowDoubleFloatArrays()).showArrays( scene_slices,
scene_slices, iwidth,
iwidth, iheight,
iheight, true,
true, dbg_title,
dbg_title, dbg_titles);
dbg_titles); }
}
// copy rest of the data (all but strength) where strength > 0
// copy rest of the data (all but strength) where strength > 0 for (int iY = 0; iY < iheight; iY++) {
for (int iY = 0; iY < iheight; iY++) { int tY = imin_tY + iY;
int tY = imin_tY + iY; if ((tY >= 0) && (tY < tilesY)) {
if ((tY >= 0) && (tY < tilesY)) { for (int iX = 0; iX < iwidth; iX++) {
for (int iX = 0; iX < iwidth; iX++) { int tX = imin_tX + iX;
int tX = imin_tX + iX; if ((tX >= 0) && (tX < tilesX)) {
if ((tX >= 0) && (tX < tilesX)) { int iTile = iX + iY * iwidth;
int iTile = iX + iY * iwidth; int tile = tX + tilesX * tY;
int tile = tX + tilesX * tY; if (scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] > 0.0) {
if (scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] > 0.0) { for (int i = 0; i < scene_slices.length; i++) if (i != QuadCLT.DSRBG_STRENGTH) {
for (int i = 0; i < scene_slices.length; i++) if (i != QuadCLT.DSRBG_STRENGTH) { scene_slices[i][iTile] = dsrbg_scene[i][tile];
scene_slices[i][iTile] = dsrbg_scene[i][tile]; }
} }
} }
} }
} }
} }
} if ((debug_level>1) && (iMTile == dbg_mtile)) {
if ((debug_level>1) && (iMTile == dbg_mtile)) { String [] dbg_titles= {"d","s","r","b","g"};
String [] dbg_titles= {"d","s","r","b","g"}; String dbg_title= "scene1-MX"+mtileX+"_MY"+mtileY;
String dbg_title= "scene1-MX"+mtileX+"_MY"+mtileY; (new ShowDoubleFloatArrays()).showArrays(
(new ShowDoubleFloatArrays()).showArrays( scene_slices,
scene_slices, iwidth,
iwidth, iheight,
iheight, true,
true, dbg_title,
dbg_title, dbg_titles);
dbg_titles);
}
// set NAN everywhere where strength is 0 (including border tiles
int num_dead = 0;
for (int iTile = 0; iTile < scene_slices[0].length; iTile++) {
if (scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] <=0.0) {
for (int i = 0; i < scene_slices.length; i++) {
scene_slices[i][iTile] = Double.NaN;
}
num_dead++;
} }
}
if ((debug_level>1) && (iMTile == dbg_mtile)) { // set NAN everywhere where strength is 0 (including border tiles
System.out.println("scene2-MX"+mtileX+"_MY"+mtileY+" num_dead="+num_dead); int num_dead = 0;
String [] dbg_titles= {"d","s","r","b","g"}; for (int iTile = 0; iTile < scene_slices[0].length; iTile++) {
String dbg_title= "scene2-MX"+mtileX+"_MY"+mtileY; if (scene_slices[QuadCLT.DSRBG_STRENGTH][iTile] <=0.0) {
(new ShowDoubleFloatArrays()).showArrays( for (int i = 0; i < scene_slices.length; i++) {
scene_slices, scene_slices[i][iTile] = Double.NaN;
iwidth,
iheight,
true,
dbg_title,
dbg_titles);
}
//center_occupancy -> all occupancy
double fract_active = 1.0*(scene_slices[0].length - num_dead)/scene_slices[0].length; // all tiles, not center
if (fract_active < occupancy) {
flowXY_frac[iMTile] = null;
continue;
}
// Here need to fill NaNs, then
tilesFillNaN(
neibw, // final double [] neibw,
scene_slices, // final double [][] slices,
num_passes, // final int num_passes,
max_change, // final double max_change,
iwidth); // final int width
if ((debug_level>1) && (iMTile == dbg_mtile)) {
String [] dbg_titles= {"d","s","r","b","g"};
String dbg_title= "scene3NaN-MX"+mtileX+"_MY"+mtileY;
(new ShowDoubleFloatArrays()).showArrays(
scene_slices,
iwidth,
iheight,
true,
dbg_title,
dbg_titles);
}
// bi-linear interpolate
double [][] scene_mapped = new double [dsrbg_scene.length][fullTileLen];
boolean need_nan_filter = false;;
for (int iY = 0; iY < fullTileSize; iY++) {
for (int iX = 0; iX < fullTileSize; iX++) {
int iTile = iX + iY * fullTileSize;
if (tXtYD[iTile] != null) {
int itX = (int) Math.floor(tXtYD[iTile][0]);
int itY = (int) Math.floor(tXtYD[iTile][1]);
double kX = tXtYD[iTile][0]-itX;
double kY = tXtYD[iTile][1]-itY;
itX -= imin_tX; // relative to scene_slices = new double [dsrbg_scene.length][iwidth*iheight];
itY -= imin_tY; // relative to scene_slices = new double [dsrbg_scene.length][iwidth*iheight];
int indx = itX + itY * iwidth;
for (int i = 0; i < scene_mapped.length; i++) {
scene_mapped[i][iTile] =
(1.0 - kY) * ((1.0-kX) * scene_slices[i][indx] + kX * scene_slices[i][indx + 1])+
( kY) * ((1.0-kX) * scene_slices[i][indx + iwidth] + kX * scene_slices[i][indx + iwidth + 1]);
}
} else {
for (int i = 0; i < scene_mapped.length; i++) {
scene_mapped[i][iTile] = Double.NaN; // will need to filter?
} }
need_nan_filter=true; num_dead++;
} }
} }
} if ((debug_level>1) && (iMTile == dbg_mtile)) {
if ((debug_level>1) && (iMTile == dbg_mtile)) { System.out.println("scene2-MX"+mtileX+"_MY"+mtileY+" num_dead="+num_dead);
String [] dbg_titles= {"d","s","r","b","g"}; String [] dbg_titles= {"d","s","r","b","g"};
String dbg_title= "mapped-MX"+mtileX+"_MY"+mtileY; String dbg_title= "scene2-MX"+mtileX+"_MY"+mtileY;
(new ShowDoubleFloatArrays()).showArrays( (new ShowDoubleFloatArrays()).showArrays(
scene_mapped, scene_slices,
fullTileSize, iwidth,
fullTileSize, iheight,
true, true,
dbg_title, dbg_title,
dbg_titles); dbg_titles);
} }
if (need_nan_filter) {
//center_occupancy -> all occupancy
double fract_active = 1.0*(scene_slices[0].length - num_dead)/scene_slices[0].length; // all tiles, not center
if (fract_active < occupancy) {
flowXY_frac[iMTile] = null;
continue;
}
// Here need to fill NaNs, then
tilesFillNaN( tilesFillNaN(
neibw, // final double [] neibw, neibw, // final double [] neibw,
scene_mapped, // final double [][] slices, scene_slices, // final double [][] slices,
num_passes, // final int num_passes, num_passes, // final int num_passes,
max_change, // final double max_change, max_change, // final double max_change,
fullTileSize); // final int width iwidth); // final int width
if ((debug_level>1) && (iMTile == dbg_mtile)) { if ((debug_level>1) && (iMTile == dbg_mtile)) {
String [] dbg_titles= {"d","s","r","b","g"}; String [] dbg_titles= {"d","s","r","b","g"};
String dbg_title= "mappedNaN-MX"+mtileX+"_MY"+mtileY; String dbg_title= "scene3NaN-MX"+mtileX+"_MY"+mtileY;
(new ShowDoubleFloatArrays()).showArrays(
scene_slices,
iwidth,
iheight,
true,
dbg_title,
dbg_titles);
}
// bi-linear interpolate
double [][] scene_mapped = new double [dsrbg_scene.length][fullTileLen];
boolean need_nan_filter = false;;
for (int iY = 0; iY < fullTileSize; iY++) {
for (int iX = 0; iX < fullTileSize; iX++) {
int iTile = iX + iY * fullTileSize;
if (tXtYD[iTile] != null) {
int itX = (int) Math.floor(tXtYD[iTile][0]);
int itY = (int) Math.floor(tXtYD[iTile][1]);
double kX = tXtYD[iTile][0]-itX;
double kY = tXtYD[iTile][1]-itY;
itX -= imin_tX; // relative to scene_slices = new double [dsrbg_scene.length][iwidth*iheight];
itY -= imin_tY; // relative to scene_slices = new double [dsrbg_scene.length][iwidth*iheight];
int indx = itX + itY * iwidth;
for (int i = 0; i < scene_mapped.length; i++) {
scene_mapped[i][iTile] =
(1.0 - kY) * ((1.0-kX) * scene_slices[i][indx] + kX * scene_slices[i][indx + 1])+
( kY) * ((1.0-kX) * scene_slices[i][indx + iwidth] + kX * scene_slices[i][indx + iwidth + 1]);
}
} else {
for (int i = 0; i < scene_mapped.length; i++) {
scene_mapped[i][iTile] = Double.NaN; // will need to filter?
}
need_nan_filter=true;
}
}
}
if ((debug_level>1) && (iMTile == dbg_mtile)) {
String [] dbg_titles= {"d","s","r","b","g"};
String dbg_title= "mapped-MX"+mtileX+"_MY"+mtileY;
(new ShowDoubleFloatArrays()).showArrays( (new ShowDoubleFloatArrays()).showArrays(
scene_mapped, scene_mapped,
fullTileSize, fullTileSize,
...@@ -854,10 +1028,29 @@ public class OpticalFlow { ...@@ -854,10 +1028,29 @@ public class OpticalFlow {
dbg_title, dbg_title,
dbg_titles); dbg_titles);
} }
if (need_nan_filter) {
tilesFillNaN(
neibw, // final double [] neibw,
scene_mapped, // final double [][] slices,
num_passes, // final int num_passes,
max_change, // final double max_change,
fullTileSize); // final int width
if ((debug_level>1) && (iMTile == dbg_mtile)) {
String [] dbg_titles= {"d","s","r","b","g"};
String dbg_title= "mappedNaN-MX"+mtileX+"_MY"+mtileY;
(new ShowDoubleFloatArrays()).showArrays(
scene_mapped,
fullTileSize,
fullTileSize,
true,
dbg_title,
dbg_titles);
}
}
scene_tiles[iMTile] = scene_mapped;
} }
scene_tiles[iMTile] = scene_mapped;
}
} }
}; };
} }
...@@ -1398,6 +1591,9 @@ public class OpticalFlow { ...@@ -1398,6 +1591,9 @@ public class OpticalFlow {
double [][] flowXY = new double [reference_tiles.length][2]; // zero pre-shifts double [][] flowXY = new double [reference_tiles.length][2]; // zero pre-shifts
double [][] flowXY_frac = new double [reference_tiles.length][]; // Will contain fractional X/Y shift for CLT double [][] flowXY_frac = new double [reference_tiles.length][]; // Will contain fractional X/Y shift for CLT
double [] chn_weights = {1.0,1.0,1.0,1.0}; // strength, r,b,g double [] chn_weights = {1.0,1.0,1.0,1.0}; // strength, r,b,g
// double [] chn_weights = {1.0,0.0,0.0,0.0}; // strength, r,b,g
// double [] chn_weights = {0.0,1.0,1.0,1.0}; // strength, r,b,g
// Apply DOG to colors, normalize by standard deviation?
double corr_sigma = 0.5; double corr_sigma = 0.5;
double fat_zero = 0.05; double fat_zero = 0.05;
double frac_radius = 0.9; // add to integer radius for window calculation double frac_radius = 0.9; // add to integer radius for window calculation
...@@ -1406,7 +1602,78 @@ public class OpticalFlow { ...@@ -1406,7 +1602,78 @@ public class OpticalFlow {
int iradius = 3; // half-size of the square to process int iradius = 3; // half-size of the square to process
double dradius = 1.5; // weight calculation (1/(r/dradius)^2 + 1) double dradius = 1.5; // weight calculation (1/(r/dradius)^2 + 1)
int refine_num = 5; // number of iterations to apply weights around new center int refine_num = 5; // number of iterations to apply weights around new center
int max_rad = 3;
boolean combine_empty_only = true; // false;
double magic_scale = 0.85;
boolean late_normalize_iterate = true;
int num_run_all = 2; // run all tiles for few iterations before filtering
int max_tries = 100;
// for recalculateFlowXY()
double min_change = 0.001;// sqrt (dx*dx + dy*dy) for correction (int tiles)
// double [] abs_change = new double [reference_tiles.length]; // updated
int debug_level_iterate = 1;
int transform_size = tp.getTileSize();
int macroTilesX = tilesX/transform_size;
int macroTilesY = tilesY/transform_size;
// public double [][]
correlate2DIterate( // returns optical flow and confidence
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
// for prepareSceneTiles()
camera_xyz0, // final double [] scene_xyz, // camera center in world coordinates
camera_atr0, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
// flowXY should be initialized to all pairs of zeros (or deliberate pixel offset pairs if initial error is too high, will be modified with each iteration
flowXY, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions // initialize to [reference_tiles.length][2]
tolerance_absolute_inter, // final double tolerance_absolute, // absolute disparity half-range in each tile
tolerance_relative_inter, // final double tolerance_relative, // relative disparity half-range in each tile
occupancy_inter, // final double occupancy, // fraction of remaining tiles (<1.0)
num_passes, // final int num_passes,
max_change, // final double max_change,
// for correlate2DSceneToReference ()
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero,
late_normalize_iterate, // final boolean late_normalize,
// for correlation2DToVectors_CM()
iradius, // final int iradius, // half-size of the square to process
dradius, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
refine_num, // final int refine_num, // number of iterations to apply weights around new center
num_run_all, // final int num_run_all, // run all tiles for few iterations before filtering
max_tries, // final int max_tries,
// for recalculateFlowXY()
// abs_change, // final double [] abs_change, // updated
magic_scale, // final double magic_scale, // 0.85 for CM
min_change, // final double min_change,
debug_level_iterate); // final int debug_level)
if (debug_level > -1) {
String dbg_title = "dXdY-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
String [] dbg_titles = {"dpX", "dpY"};
double [][] dbg_img = new double [dbg_titles.length][macroTilesX*macroTilesY];
Arrays.fill(dbg_img[0], Double.NaN);
Arrays.fill(dbg_img[1], Double.NaN);
for (int i = 0; i < flowXY.length; i++) if (flowXY[i] != null){
dbg_img[0][i] = flowXY[i][0];
dbg_img[1][i] = flowXY[i][1];
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
macroTilesX,
macroTilesY,
true,
dbg_title,
dbg_titles);
}
double [][][] scene_tiles = prepareSceneTiles(// to match to reference double [][][] scene_tiles = prepareSceneTiles(// to match to reference
// null for {scene,reference}{xyz,atr} uses instances globals // null for {scene,reference}{xyz,atr} uses instances globals
...@@ -1424,10 +1691,7 @@ public class OpticalFlow { ...@@ -1424,10 +1691,7 @@ public class OpticalFlow {
max_change, // final double max_change, max_change, // final double max_change,
1); //-1); // 1); // 2); // final int debug_level) 1); //-1); // 1); // 2); // final int debug_level)
/* */ /* */
int max_rad = 3;
double [][][] corr2dscene_ref_multi = new double [max_rad + 2][][]; double [][][] corr2dscene_ref_multi = new double [max_rad + 2][][];
// double [][] corr2dscene_ref_0
corr2dscene_ref_multi[0] = correlate2DSceneToReference(// to match to reference corr2dscene_ref_multi[0] = correlate2DSceneToReference(// to match to reference
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
...@@ -1440,35 +1704,31 @@ public class OpticalFlow { ...@@ -1440,35 +1704,31 @@ public class OpticalFlow {
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g) chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma, corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero, fat_zero, // final double fat_zero,
false, // final boolean late_normalize, false, // final boolean late_normalize,
combine_empty_only, // final boolean combine_empty_only, // only use neighbor correlations for empty tiles (false - any)
0.0, // final double combine_dradius 0.0, // final double combine_dradius
tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles
-1); // 1); // final int debug_level) -1); // 1); // final int debug_level)
for (int irad = 0; irad <= 3; irad++) { for (int irad = 0; irad <= 3; irad++) {
// double [][] corr2dscene_ref_1
corr2dscene_ref_multi[irad+1]= correlate2DSceneToReference(// to match to reference corr2dscene_ref_multi[irad+1]= correlate2DSceneToReference(// to match to reference
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
scene_QuadCLT, // final QuadCLT scene_QuadClt, scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt, reference_QuadCLT, // final QuadCLT reference_QuadClt,
scene_tiles, // final double [][][] scene_tiles, // prepared with prepareSceneTiles() scene_tiles, // final double [][][] scene_tiles, // prepared with prepareSceneTiles()
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine? reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine?
// final double [][] flowXY, // per macro tile {mismatch in IMAGE_PIXELS in X and Y directions (pre-shift)
flowXY_frac, // final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations flowXY_frac, // final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g) chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma, corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero, fat_zero, // final double fat_zero,
true, // final boolean late_normalize, true, // final boolean late_normalize,
combine_empty_only, // final boolean combine_empty_only, // only use neighbor correlations for empty tiles (false - any)
irad + frac_radius, // final double combine_dradius irad + frac_radius, // final double combine_dradius
tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles
-1); // 1); // final int debug_level) -1); // 1); // final int debug_level)
} }
int transform_size = tp.getTileSize();
int macroTilesX = tilesX/transform_size;
int macroTilesY = tilesY/transform_size;
// double [][][] multicorrs = new double[][][] {corr2dscene_ref_0, corr2dscene_ref_1};
showCorrTiles( showCorrTiles(
"scene:"+scene_QuadCLT.getImageName()+"-ref"+reference_QuadCLT.getImageName(), // String title, "scene:"+scene_QuadCLT.getImageName()+"-ref"+reference_QuadCLT.getImageName(), // String title,
...@@ -1477,44 +1737,173 @@ public class OpticalFlow { ...@@ -1477,44 +1737,173 @@ public class OpticalFlow {
(2 * transform_size - 1), // int tile_width, (2 * transform_size - 1), // int tile_width,
(2 * transform_size - 1)); // int tile_height) // extra margins over 16x16 tiles to accommodate distorted destination tiles (2 * transform_size - 1)); // int tile_height) // extra margins over 16x16 tiles to accommodate distorted destination tiles
double [][][] vectorsXYS = new double [corr2dscene_ref_multi.length][][];
for (int i = 0; i < vectorsXYS.length; i++) { if (debug_level > 100) {
vectorsXYS[i] = correlation2DToVectors_CM( double [][][] vectorsXYS = new double [corr2dscene_ref_multi.length][][];
corr2dscene_ref_multi[i], // final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls) for (int i = 0; i < vectorsXYS.length; i++) {
transform_size, // final int transform_size, vectorsXYS[i] = correlation2DToVectors_CM(
iradius, // final int iradius, // half-size of the square to process corr2dscene_ref_multi[i], // final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls)
dradius, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1) transform_size, // final int transform_size,
refine_num, // final int refine_num, // number of iterations to apply weights around new center iradius, // final int iradius, // half-size of the square to process
1); //final int debug_level) dradius, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
} refine_num, // final int refine_num, // number of iterations to apply weights around new center
if (debug_level > -1) { 1); //final int debug_level)
String dbg_title = "dXdYS-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName(); }
int slices = vectorsXYS.length; if (debug_level > -1) {
String [] dbg_titles = new String[slices * 3]; String dbg_title = "dXdYS-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
double [][] dbg_img = new double [dbg_titles.length][macroTilesX*macroTilesY]; int slices = vectorsXYS.length;
for (int slice = 0; slice < vectorsXYS.length; slice++) { String [] dbg_titles = new String[slices * 3];
dbg_titles[slice + 0 * slices] = "dX"+slice; double [][] dbg_img = new double [dbg_titles.length][macroTilesX*macroTilesY];
dbg_titles[slice + 1 * slices] = "dY"+slice; for (int slice = 0; slice < vectorsXYS.length; slice++) {
dbg_titles[slice + 2 * slices] = "S"+slice; dbg_titles[slice + 0 * slices] = "dX"+slice;
Arrays.fill(dbg_img[slice + slices * 0], Double.NaN); dbg_titles[slice + 1 * slices] = "dY"+slice;
Arrays.fill(dbg_img[slice + slices * 1], Double.NaN); dbg_titles[slice + 2 * slices] = "S"+slice;
Arrays.fill(dbg_img[slice + slices * 2], Double.NaN); Arrays.fill(dbg_img[slice + slices * 0], Double.NaN);
for (int i = 0; i < vectorsXYS[slice].length; i++) if (vectorsXYS[slice][i] != null){ Arrays.fill(dbg_img[slice + slices * 1], Double.NaN);
dbg_img[slice + 0 * slices][i] = vectorsXYS[slice][i][0]; Arrays.fill(dbg_img[slice + slices * 2], Double.NaN);
dbg_img[slice + 1 * slices][i] = vectorsXYS[slice][i][1]; for (int i = 0; i < vectorsXYS[slice].length; i++) if (vectorsXYS[slice][i] != null){
dbg_img[slice + 2 * slices][i] = vectorsXYS[slice][i][2]; dbg_img[slice + 0 * slices][i] = vectorsXYS[slice][i][0];
dbg_img[slice + 1 * slices][i] = vectorsXYS[slice][i][1];
dbg_img[slice + 2 * slices][i] = vectorsXYS[slice][i][2];
}
} }
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
macroTilesX,
macroTilesY,
true,
dbg_title,
dbg_titles);
} }
(new ShowDoubleFloatArrays()).showArrays( int selected_index = 1; // single, post-norm
dbg_img, double [][] flowXY1 = recalculateFlowXY(
macroTilesX, flowXY, // final double [][] currentFlowXY,
macroTilesY, vectorsXYS[selected_index], // final double [][] corr_vectorsXY,
true, magic_scale/transform_size); // final double magic_scale) // 0.85 for CM
dbg_title,
dbg_titles); double [][][] scene_tiles1 = prepareSceneTiles(// to match to reference
// null for {scene,reference}{xyz,atr} uses instances globals
camera_xyz0, // final double [] scene_xyz, // camera center in world coordinates
camera_atr0, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
flowXY1, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions
flowXY_frac, // final double [][] flowXY_frac, // should be initialized as [number of macro tiles][] - returns fractional shifts [-0.5, 0.5)
tolerance_absolute_inter, // final double tolerance_absolute, // absolute disparity half-range in each tile
tolerance_relative_inter, // final double tolerance_relative, // relative disparity half-range in each tile
occupancy_inter, // final double occupancy, // fraction of remaining tiles (<1.0)
num_passes, // final int num_passes,
max_change, // final double max_change,
1); //-1); // 1); // 2); // final int debug_level)
// single, late
corr2dscene_ref_multi[0] = correlate2DSceneToReference(// to match to reference
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
scene_tiles1, // final double [][][] scene_tiles, // prepared with prepareSceneTiles()
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine?
flowXY_frac, // final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero,
false, // final boolean late_normalize,
combine_empty_only, // final boolean combine_empty_only, // only use neighbor correlations for empty tiles (false - any)
0.0, // final double combine_dradius
tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles
-1); // 1); // final int debug_level)
for (int irad = 0; irad <= 3; irad++) {
corr2dscene_ref_multi[irad+1]= correlate2DSceneToReference(// to match to reference
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
scene_tiles1, // final double [][][] scene_tiles, // prepared with prepareSceneTiles()
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans(); - combine?
flowXY_frac, // final double [][] flowXY_frac, // X, YH fractional shift [-0.5,0.5) to implement with FD rotations
chn_weights, // final double [] chn_weights, // absolute, starting from strength (strength,r,b,g)
corr_sigma, // final double corr_sigma,
fat_zero, // final double fat_zero,
true, // final boolean late_normalize,
combine_empty_only, // final boolean combine_empty_only, // only use neighbor correlations for empty tiles (false - any)
irad + frac_radius, // final double combine_dradius
tolerance_absolute_inter_macro, // final double tolerance_absolute, // absolute disparity half-range to consolidate tiles
tolerance_relative_inter_macro, // final double tolerance_relative, // relative disparity half-range to consolidate tiles
-1); // 1); // final int debug_level)
}
showCorrTiles(
"scene:"+scene_QuadCLT.getImageName()+"-ref"+reference_QuadCLT.getImageName(), // String title,
corr2dscene_ref_multi, // double [][] source_tiles,
tilesX/transform_size, // int tilesX,
(2 * transform_size - 1), // int tile_width,
(2 * transform_size - 1)); // int tile_height) // extra margins over 16x16 tiles to accommodate distorted destination tiles
double [][][] vectorsXYS1 = new double [corr2dscene_ref_multi.length][][];
for (int i = 0; i < vectorsXYS1.length; i++) {
vectorsXYS1[i] = correlation2DToVectors_CM(
corr2dscene_ref_multi[i], // final double [][] corr2d_tiles, // per 2d calibration tiles (or nulls)
transform_size, // final int transform_size,
iradius, // final int iradius, // half-size of the square to process
dradius, // final double dradius, // weight calculation (1/(r/dradius)^2 + 1)
refine_num, // final int refine_num, // number of iterations to apply weights around new center
1); //final int debug_level)
}
if (debug_level > -1) {
String dbg_title = "dXdYS1-"+scene_QuadCLT.getImageName()+"-"+reference_QuadCLT.getImageName();
int slices = vectorsXYS1.length;
String [] dbg_titles = new String[slices * 3];
double [][] dbg_img = new double [dbg_titles.length][macroTilesX*macroTilesY];
for (int slice = 0; slice < vectorsXYS1.length; slice++) {
dbg_titles[slice + 0 * slices] = "dX"+slice;
dbg_titles[slice + 1 * slices] = "dY"+slice;
dbg_titles[slice + 2 * slices] = "S"+slice;
Arrays.fill(dbg_img[slice + slices * 0], Double.NaN);
Arrays.fill(dbg_img[slice + slices * 1], Double.NaN);
Arrays.fill(dbg_img[slice + slices * 2], Double.NaN);
for (int i = 0; i < vectorsXYS1[slice].length; i++) if (vectorsXYS1[slice][i] != null){
dbg_img[slice + 0 * slices][i] = vectorsXYS1[slice][i][0];
dbg_img[slice + 1 * slices][i] = vectorsXYS1[slice][i][1];
dbg_img[slice + 2 * slices][i] = vectorsXYS1[slice][i][2];
}
}
(new ShowDoubleFloatArrays()).showArrays(
dbg_img,
macroTilesX,
macroTilesY,
true,
dbg_title,
dbg_titles);
}
double [][] flowXY2 = recalculateFlowXY(
flowXY1, // final double [][] currentFlowXY,
vectorsXYS1[selected_index], // final double [][] corr_vectorsXY,
magic_scale/transform_size); // final double magic_scale) // 0.85 for CM
double [][][] scene_tiles2 = prepareSceneTiles(// to match to reference
// null for {scene,reference}{xyz,atr} uses instances globals
camera_xyz0, // final double [] scene_xyz, // camera center in world coordinates
camera_atr0, // final double [] scene_atr, // camera orientation relative to world frame
scene_QuadCLT, // final QuadCLT scene_QuadClt,
reference_QuadCLT, // final QuadCLT reference_QuadClt,
reference_tiles, // final double [][][] reference_tiles, // prepared with prepareReferenceTiles() + fillTilesNans();
flowXY2, // final double [][] flowXY, // per macro tile {mismatch in image pixels in X and Y directions
flowXY_frac, // final double [][] flowXY_frac, // should be initialized as [number of macro tiles][] - returns fractional shifts [-0.5, 0.5)
tolerance_absolute_inter, // final double tolerance_absolute, // absolute disparity half-range in each tile
tolerance_relative_inter, // final double tolerance_relative, // relative disparity half-range in each tile
occupancy_inter, // final double occupancy, // fraction of remaining tiles (<1.0)
num_passes, // final int num_passes,
max_change, // final double max_change,
1); //-1); // 1); // 2); // final int debug_level)
} }
return pair; return pair;
} }
......
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