Commit e824a17c authored by Andrey Filippov's avatar Andrey Filippov

CODEX: Route lean pose getFxDerivs through CUDA

Co-authored-by: 's avatarCodex <codex@elphel.com>
parent 95e72b22
...@@ -834,6 +834,8 @@ public class CuasPoseRT { ...@@ -834,6 +834,8 @@ public class CuasPoseRT {
private static boolean peaks_path_reported = false; private static boolean peaks_path_reported = false;
// once-per-program console note for the rung-3 normal-equation product path // once-per-program console note for the rung-3 normal-equation product path
private static boolean lma_products_path_reported = false; private static boolean lma_products_path_reported = false;
// once-per-program console note for production getFxDerivs() on CUDA
private static boolean pose_fx_path_reported = false;
// one raw-pixel/J comparison of the float oracle against the current double geometry // one raw-pixel/J comparison of the float oracle against the current double geometry
private static boolean float_jacobian_oracle_reported = false; private static boolean float_jacobian_oracle_reported = false;
...@@ -1015,6 +1017,21 @@ public class CuasPoseRT { ...@@ -1015,6 +1017,21 @@ public class CuasPoseRT {
clt_parameters.ilp.ilma_thread_invariant, clt_parameters.ilp.ilma_thread_invariant,
0.0); // no disparity weight (2D only) 0.0); // no disparity weight (2D only)
final GpuQuad lmaGpu = center_CLT.getGPUQuad(); final GpuQuad lmaGpu = center_CLT.getGPUQuad();
intersceneLma.setPoseFxProvider((
reference, sceneCamera,
referenceXyz, referenceAtr, sceneXyz, sceneAtr,
centers, poseSelection, calculateJacobian) -> {
final IntersceneLmaFloat.Result result = lmaGpu.execPoseFxJacobian(
reference, sceneCamera,
referenceXyz, referenceAtr, sceneXyz, sceneAtr,
centers, poseSelection, calculateJacobian);
if ((result != null) && !pose_fx_path_reported) {
pose_fx_path_reported = true;
System.out.println("CuasPoseRT: CUDA pose getFxDerivs active "+
"(float raw fx/J with per-call H2D/D2H; Java LMA policy retained)");
}
return result;
}); // first production gate: replace only getFxDerivs per-tile work; callers remain unchanged // By Codex on 07/15/2026
intersceneLma.setNormalEquationProvider((weights, jt, ymfxWeighted) -> { intersceneLma.setNormalEquationProvider((weights, jt, ymfxWeighted) -> {
final long profileStart = (rtProfile != null) ? rtProfile.start() : 0L; final long profileStart = (rtProfile != null) ? rtProfile.start() : 0L;
final double [] products = lmaGpu.execLmaNormalProducts(weights, jt, ymfxWeighted); final double [] products = lmaGpu.execLmaNormalProducts(weights, jt, ymfxWeighted);
......
...@@ -85,6 +85,25 @@ public class IntersceneLma { ...@@ -85,6 +85,25 @@ public class IntersceneLma {
double [] products(double [] weights, double [][] jt, double [] ymfxWeighted); double [] products(double [] weights, double [][] jt, double [] ymfxWeighted);
} }
private NormalEquationProvider normalEquationProvider = null; private NormalEquationProvider normalEquationProvider = null;
/** Optional primitive-float backend for the expensive per-tile pose projection/Jacobian. */
@FunctionalInterface
public interface PoseFxProvider {
IntersceneLmaFloat.Result calculate(
IntersceneLmaFloat.Camera reference,
IntersceneLmaFloat.Camera scene,
float [] referenceXyz,
float [] referenceAtr,
float [] sceneXyz,
float [] sceneAtr,
float [] centers,
boolean [] selection,
boolean calculateJacobian);
}
private PoseFxProvider poseFxProvider = null;
private IntersceneLmaFloat.Camera poseFloatReference = null;
private IntersceneLmaFloat.Camera poseFloatScene = null;
private float [] poseFloatCenters = null;
private boolean [] poseFloatSelection = null;
public IntersceneLma( public IntersceneLma(
boolean thread_invariant, boolean thread_invariant,
...@@ -102,6 +121,16 @@ public class IntersceneLma { ...@@ -102,6 +121,16 @@ public class IntersceneLma {
public void setNormalEquationProvider(final NormalEquationProvider provider) { public void setNormalEquationProvider(final NormalEquationProvider provider) {
this.normalEquationProvider = provider; this.normalEquationProvider = provider;
} }
/** Install a raw three-angle float pose fx/J backend. Null restores the Java double path. */
public void setPoseFxProvider(final PoseFxProvider provider) {
this.poseFxProvider = provider;
}
private void resetPoseFxInputs() {
poseFloatReference = null;
poseFloatScene = null;
poseFloatCenters = null;
poseFloatSelection = null;
}
public double [][] getLastJT(){ public double [][] getLastJT(){
return last_jt; return last_jt;
} }
...@@ -361,6 +390,7 @@ public class IntersceneLma { ...@@ -361,6 +390,7 @@ public class IntersceneLma {
eigen); // final double [][] eigen); // [tilesX*tilesY]{lamb0_x,lamb0_y, lamb0, lamb1} eigenvector0[x,y],lam0,lam1 eigen); // final double [][] eigen); // [tilesX*tilesY]{lamb0_x,lamb0_y, lamb0, lamb1} eigenvector0[x,y],lam0,lam1
tilesX = reference_QuadClt.getTileProcessor().getTilesX(); // just for debug images tilesX = reference_QuadClt.getTileProcessor().getTilesX(); // just for debug images
scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt}; scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
resetPoseFxInputs();
par_mask = param_select; par_mask = param_select;
macrotile_centers = centers; macrotile_centers = centers;
num_samples = num_components * centers.length; num_samples = num_components * centers.length;
...@@ -509,6 +539,7 @@ public class IntersceneLma { ...@@ -509,6 +539,7 @@ public class IntersceneLma {
final int debug_level) final int debug_level)
{ {
scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt}; scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
resetPoseFxInputs();
par_mask = param_select; par_mask = param_select;
macrotile_centers = centers; macrotile_centers = centers;
num_samples = num_components * centers.length; num_samples = num_components * centers.length;
...@@ -652,6 +683,7 @@ public class IntersceneLma { ...@@ -652,6 +683,7 @@ public class IntersceneLma {
return false; return false;
} }
scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt}; scenesCLT = new QuadCLT [] {reference_QuadClt, scene_QuadClt};
resetPoseFxInputs();
final ErsCorrection ers_ref = reference_QuadClt.getErsCorrection(); final ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
final ErsCorrection ers_scene = scene_QuadClt.getErsCorrection(); final ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
final double [] scene_xyz = (scene_xyzatr0 != null) ? scene_xyzatr0[0] : ers_scene.camera_xyz; final double [] scene_xyz = (scene_xyzatr0 != null) ? scene_xyzatr0[0] : ers_scene.camera_xyz;
...@@ -1570,6 +1602,17 @@ public class IntersceneLma { ...@@ -1570,6 +1602,17 @@ public class IntersceneLma {
} }
ers_scene.setupERS(); ers_scene.setupERS();
ers_ref.setupERS(); ers_ref.setupERS();
if (!mb_mode && fillFromPoseFxProvider(
ers_ref, ers_scene,
reference_xyz, reference_atr, scene_xyz, scene_atr,
fx, jt)) {
// Pull rows are fixed-size policy, not per-tile projection work.
for (int i = 0; i < par_indices.length; i++) {
fx[i + num_samples] = vector[i];
if (jt != null) jt[i][i + num_samples] = 1.0;
}
return fx;
}
final Matrix [] reference_matrices_inverse = ErsCorrection.getInterRotDeriveMatrices( final Matrix [] reference_matrices_inverse = ErsCorrection.getInterRotDeriveMatrices(
reference_atr, // double [] atr); reference_atr, // double [] atr);
true); // boolean invert)); true); // boolean invert));
...@@ -1670,7 +1713,71 @@ public class IntersceneLma { ...@@ -1670,7 +1713,71 @@ public class IntersceneLma {
/// } /// }
return fx; return fx;
} }
/**
* Use the primitive backend only for the frozen lean contract: two pixel
* components and the three scene-angle parameters. All general LMA shapes
* continue through the double Java implementation below.
*/
private boolean fillFromPoseFxProvider(
final ErsCorrection ers_ref,
final ErsCorrection ers_scene,
final double [] reference_xyz,
final double [] reference_atr,
final double [] scene_xyz,
final double [] scene_atr,
final double [] fx,
final double [][] jt) {
if ((poseFxProvider == null) || (num_components != 2) ||
(par_indices == null) || (par_indices.length != IntersceneLmaFloat.NUM_PARAMS)) return false;
for (int par = 0; par < IntersceneLmaFloat.NUM_PARAMS; par++) {
if (par_indices[par] != ErsCorrection.DP_DSAZ + par) return false;
}
if (poseFloatReference == null) poseFloatReference = IntersceneLmaFloat.Camera.capture(ers_ref);
if (poseFloatScene == null) poseFloatScene = IntersceneLmaFloat.Camera.capture(ers_scene);
if (poseFloatCenters == null) poseFloatCenters = IntersceneLmaFloat.flattenCenters(macrotile_centers);
if (poseFloatSelection == null) {
poseFloatSelection = new boolean [macrotile_centers.length];
for (int tile = 0; tile < poseFloatSelection.length; tile++) {
poseFloatSelection[tile] = (macrotile_centers[tile] != null) && (weights[2 * tile] > 0.0);
}
}
final IntersceneLmaFloat.Result result = poseFxProvider.calculate(
poseFloatReference, poseFloatScene,
IntersceneLmaFloat.toFloat3(reference_xyz), IntersceneLmaFloat.toFloat3(reference_atr),
IntersceneLmaFloat.toFloat3(scene_xyz), IntersceneLmaFloat.toFloat3(scene_atr),
poseFloatCenters, poseFloatSelection, jt != null);
if (result == null) return false;
if ((result.fx == null) || (result.valid == null) ||
(result.numTiles != macrotile_centers.length) ||
(result.fx.length != 2 * result.numTiles) ||
(result.valid.length != result.numTiles) ||
((jt != null) && ((result.jt == null) ||
(result.jt.length != IntersceneLmaFloat.NUM_PARAMS * 2 * result.numTiles)))) {
throw new IllegalStateException("IntersceneLma: pose fx/J backend returned inconsistent arrays");
}
for (int tile = 0; tile < result.numTiles; tile++) {
if (result.valid[tile] == 0) continue; // legacy geometry leaves zero rows for an unusable LMA tile
final int vi = 2 * tile;
fx[vi] = result.fx[vi];
fx[vi + 1] = result.fx[vi + 1];
if (jt == null) continue;
for (int par = 0; par < IntersceneLmaFloat.NUM_PARAMS; par++) {
final int ji = par * 2 * result.numTiles + vi;
final float raw_x = result.jt[ji];
final float raw_y = result.jt[ji + 1];
if (eig_trans == null) {
jt[par][vi] = raw_x;
jt[par][vi + 1] = raw_y;
} else {
jt[par][vi] = eig_trans[tile][0][0] * raw_x + eig_trans[tile][0][1] * raw_y;
jt[par][vi + 1] = eig_trans[tile][1][0] * raw_x + eig_trans[tile][1][1] * raw_y;
}
}
}
return true;
}
private double [][] getWJtJlambda( // USED in lwir private double [][] getWJtJlambda( // USED in lwir
final double lambda, final double lambda,
final double [][] jt) final double [][] jt)
......
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