Commit e7771dbc authored by Andrey Filippov's avatar Andrey Filippov

CODEX: Add float pose Jacobian oracle

Co-authored-by: 's avatarCodex <codex@elphel.com>
parent 0259ecad
......@@ -40,6 +40,7 @@ import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.Interscene;
import com.elphel.imagej.tileprocessor.IntersceneLma;
import com.elphel.imagej.tileprocessor.IntersceneLmaFloat;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import com.elphel.imagej.tileprocessor.QuadCLTCPU;
......@@ -833,6 +834,8 @@ public class CuasPoseRT {
private static boolean peaks_path_reported = false;
// once-per-program console note for the rung-3 normal-equation product path
private static boolean lma_products_path_reported = false;
// one raw-pixel/J comparison of the float oracle against the current double geometry
private static boolean float_jacobian_oracle_reported = false;
private static int GPUTileProcessorDttSize() {
return com.elphel.imagej.gpu.GPUTileProcessor.DTT_SIZE;
......@@ -1083,6 +1086,31 @@ public class CuasPoseRT {
null, // dbg_prefix
clt_parameters.imp.debug_level); // debug_level
if (rtProfile != null) rtProfile.addElapsed(RtPoseProfile.LMA_PREPARE, profileStart);
if ((clt_parameters.curt.pose_lma_debug >= 1) && !float_jacobian_oracle_reported) {
float_jacobian_oracle_reported = true;
final boolean [] float_selection = new boolean [cm[0].length];
for (int tile = 0; tile < float_selection.length; tile++) {
float_selection[tile] = (cm[0][tile] != null) && (cm[1][tile] != null) &&
(cm[1][tile].length > 2) && (cm[1][tile][2] > 0.0);
}
final ErsCorrection reference_ers = center_CLT.getErsCorrection();
final ErsCorrection scene_ers = scene.getErsCorrection();
final IntersceneLmaFloat.Result float_result = IntersceneLmaFloat.getFxDerivs(
IntersceneLmaFloat.Camera.capture(reference_ers),
IntersceneLmaFloat.Camera.capture(scene_ers),
IntersceneLmaFloat.toFloat3(ZERO3),
IntersceneLmaFloat.toFloat3(ZERO3),
IntersceneLmaFloat.toFloat3(scene_xyzatr0[0]),
IntersceneLmaFloat.toFloat3(scene_xyzatr0[1]),
IntersceneLmaFloat.flattenCenters(cm[0]),
float_selection,
null); // validate raw pixel/J first; eigen transform is a separate layer
final IntersceneLmaFloat.Comparison float_comparison = IntersceneLmaFloat.compareToDouble(
float_result, reference_ers, scene_ers,
ZERO3, ZERO3, scene_xyzatr0[0], scene_xyzatr0[1], cm[0],
float_selection, null, -1);
System.out.println("CuasPoseRT "+float_comparison);
}
cycle_rms_meas[nlma] = intersceneLma.getLastRms()[1]; // re-measured pure RMS at this cycle's pose // By Claude on 07/12/2026
// pose_lma_debug >= 2: per-inner-step solver lines; header attributes them to
// their outer cycle (the solver lines carry no scene/cycle context).
......
......@@ -910,6 +910,35 @@ public class ErsCorrection extends GeometryCorrection {
}
/**
* Snapshot the per-line ERS state in the primitive layout used by the float
* pose-Jacobian oracle. Each line contains xyz[3], xyz_dt[3], quaternion[4]
* and quaternion_dt[4]. Package-private on purpose: this is an implementation
* bridge for {@link IntersceneLmaFloat}, not a new public camera API.
*/
float [] copyErsPoseStateFloat()
{
setupERS();
final int stride = IntersceneLmaFloat.ERS_LINE_STRIDE;
final float [] state = new float [pixelCorrectionHeight * stride];
for (int line = 0; line < pixelCorrectionHeight; line++) {
final int base = line * stride;
for (int i = 0; i < 3; i++) {
state[base + IntersceneLmaFloat.ERS_XYZ + i] = (float) ers_xyz[line][i];
state[base + IntersceneLmaFloat.ERS_XYZ_DT + i] = (float) ers_xyz_dt[line][i];
}
state[base + IntersceneLmaFloat.ERS_QUAT + 0] = (float) ers_quaternion[line].getQ0();
state[base + IntersceneLmaFloat.ERS_QUAT + 1] = (float) ers_quaternion[line].getQ1();
state[base + IntersceneLmaFloat.ERS_QUAT + 2] = (float) ers_quaternion[line].getQ2();
state[base + IntersceneLmaFloat.ERS_QUAT + 3] = (float) ers_quaternion[line].getQ3();
state[base + IntersceneLmaFloat.ERS_QUAT_DT + 0] = (float) ers_quaternion_dt[line].getQ0();
state[base + IntersceneLmaFloat.ERS_QUAT_DT + 1] = (float) ers_quaternion_dt[line].getQ1();
state[base + IntersceneLmaFloat.ERS_QUAT_DT + 2] = (float) ers_quaternion_dt[line].getQ2();
state[base + IntersceneLmaFloat.ERS_QUAT_DT + 3] = (float) ers_quaternion_dt[line].getQ3();
}
return state;
}
/**
* Match other camera px, py, disparity to the reference one
* @param px horizontal pixel coordinate (right) of the reference camera view
......@@ -2758,4 +2787,3 @@ public class ErsCorrection extends GeometryCorrection {
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