Commit 1cece288 authored by Andrey Filippov's avatar Andrey Filippov

UAS projection fixes: center LLA persistence, ATR sign, logging

- Cuas.createCenterClt(): average LLA from sequence scenes and save to
  center IMS XML file (did_ins_2 only, no EventLogger dependency)
- QuadCLTCPU.restoreCenterClt(): load averaged LLA from center IMS file
  so addUasData() has a valid camera reference position
- CuasRanging.addUasData(): log camera ATR, LLA, uas_home_alt, ned_corr
  before per-sequence loop; add cam_az/cam_tilt/cam_roll columns to TSV
- UasLogReader: log ATR and LLA values when set; add getCameraLLA(),
  getUasHomeAlt(), getNedCorr() accessors
- TwoQuadCLT: add comment explaining deferred setCameraLLA() design
- CuasMotion: log UAS tile count and px/py/range per sequence

Root cause found: camera_atr[0] (azimuth) in .list file is the
ErsCorrection Y-frame-rotation angle, not compass bearing — they have
opposite signs. Rule: az_list = -(compass_bearing_rad). Confirmed by
comparison with working Eagle Mountain 2025 data (+2.7595 for SSW camera).
Calibrated Latvia value: 2.0396, 0.0702, 0.0.
Co-Authored-By: 's avatarClaude Sonnet 4.6 <noreply@anthropic.com>
parent dd2ce06e
......@@ -2,16 +2,20 @@ package com.elphel.imagej.cuas;
import java.awt.Rectangle;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.Properties;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.stream.IntStream;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.common.PolynomialApproximation;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.tileprocessor.ErsCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.OpticalFlow;
......@@ -20,6 +24,7 @@ import com.elphel.imagej.tileprocessor.TileProcessor;
import com.elphel.imagej.tileprocessor.TwoQuadCLT;
import com.elphel.imagej.vegetation.VegetationSegment;
import ij.IJ;
import ij.ImagePlus;
import ij.Prefs;
......@@ -486,6 +491,65 @@ public class Cuas {
// center_CLT.saveCenterClt();
//***************************************************
//FIXME: implement combine DSI!
// Inject averaged LLA into center_CLT.did_ins_2 and persist to IMS file.
// center_CLT is a virtual scene with no raw IMS logs; we average the camera
// position from the real sequence scenes and save only the did_ins_2 entry so
// that restoreCenterClt() can load it later without touching EventLogger.
{
Did_ins_2 ref_did_ins_2 = null;
double lat_sum = 0, lon_sum = 0, alt_sum = 0;
int lla_count = 0;
for (int i = range[0]; i <= range[1]; i++) {
if (quadCLTs[i] != null && quadCLTs[i].did_ins_2 != null) {
double [] lla = quadCLTs[i].did_ins_2.lla;
if (lla != null && (lla[0] != 0.0 || lla[1] != 0.0)) {
if (ref_did_ins_2 == null) ref_did_ins_2 = quadCLTs[i].did_ins_2;
lat_sum += lla[0];
lon_sum += lla[1];
alt_sum += lla[2];
lla_count++;
}
}
}
if (ref_did_ins_2 != null) {
// Clone ref_did_ins_2 (keeps quaternion/timing) and override LLA with sequence average
Properties center_ims_props = new Properties();
ref_did_ins_2.setProperties("did_ins_2-", center_ims_props);
Did_ins_2 center_did_ins_2 = new Did_ins_2("did_ins_2-", center_ims_props);
center_did_ins_2.lla[0] = lat_sum / lla_count;
center_did_ins_2.lla[1] = lon_sum / lla_count;
center_did_ins_2.lla[2] = alt_sum / lla_count;
center_CLT.did_ins_2 = center_did_ins_2;
// Write minimal IMS XML: ims_offs, gmt_plus, did_ins_2 only.
// Do not call saveIms() — it tries to populate did_ins_1/did_pimu etc. via EventLogger.
Properties ims_properties = new Properties();
ims_properties.setProperty("ims_offs", clt_parameters.imp.ims_offset + "");
ims_properties.setProperty("gmt_plus", clt_parameters.imp.gmt_plus + "");
center_did_ins_2.setProperties("did_ins_2-", ims_properties);
// Mirror saveIms() for physical scenes: store above the version subdir.
// center_CLT.getImagePath() == <center_dir>/v04, so getParent() == <center_dir>
String center_scene_dir = new File(center_CLT.getImagePath()).getParent();
String ims_path = center_scene_dir + Prefs.getFileSeparator() +
center_CLT.getImageName() + center_CLT.correctionsParameters.imsSuffix;
try (OutputStream os = new FileOutputStream(ims_path)) {
ims_properties.storeToXML(os, "last updated " + new java.util.Date(), "UTF8");
center_CLT.ims_offs = clt_parameters.imp.ims_offset;
center_CLT.gmt_plus = clt_parameters.imp.gmt_plus;
center_CLT.ims_last_path = ims_path;
} catch (IOException e) {
System.out.println("createCenterClt(): ERROR saving center IMS file: " + ims_path);
e.printStackTrace();
}
System.out.println("createCenterClt(): center_CLT did_ins_2 averaged from " + lla_count +
" scenes: lat=" + IJ.d2s(center_did_ins_2.lla[0], 6) +
" lon=" + IJ.d2s(center_did_ins_2.lla[1], 6) +
" alt=" + IJ.d2s(center_did_ins_2.lla[2], 1) + ", saved to " + ims_path);
} else {
System.out.println("createCenterClt(): WARNING: no valid LLA in sequence scenes, center_CLT.did_ins_2 not set");
}
}
return center_CLT;
}
......
......@@ -2845,7 +2845,22 @@ public class CuasMotion {
for (int nseq = 0; nseq < num_seq; nseq++) if ((local_tiles[nseq] != null) && (local_tiles[nseq].length > 0)){ // no if is not needed - bug fixed
uas_tiles[nseq] = local_tiles[nseq][0];
}
{
int uas_count = 0;
for (int nseq = 0; nseq < num_seq; nseq++) if (uas_tiles[nseq] >= 0) uas_count++;
System.out.println("convertToRgbAnnotateTargets(): "+uas_count+"/"+num_seq+
" sequences have UAS flight log data (need >=2 consecutive for circle)");
if (uas_count > 0) {
for (int nseq = 0; nseq < num_seq; nseq++) if (uas_tiles[nseq] >= 0) {
double [] t = targets[nseq][uas_tiles[nseq]];
System.out.println(" seq="+nseq+" tile="+uas_tiles[nseq]+
" px="+IJ.d2s(t[CuasMotionLMA.RSLT_FL_PX],1)+
" py="+IJ.d2s(t[CuasMotionLMA.RSLT_FL_PY],1)+
" range="+IJ.d2s(t[CuasMotionLMA.RSLT_FL_RANGE],1));
}
}
}
for (int ltarg = 1; ltarg <= ltargets_first_last.length; ltarg++) { // 0 - for UAS log
int indx_first, indx_last;
for (indx_first = 0; (indx_first < num_seq) && ((local_tiles[indx_first].length <= ltarg ) || (local_tiles[indx_first][ltarg] < 0)); indx_first++);
......
......@@ -20,6 +20,7 @@ import com.elphel.imagej.tileprocessor.ImageDttParameters;
import com.elphel.imagej.tileprocessor.OpticalFlow;
import com.elphel.imagej.tileprocessor.QuadCLT;
import ij.IJ;
import ij.ImagePlus;
import ij.Prefs;
......@@ -2707,19 +2708,52 @@ public class CuasRanging {
return masked_img;
}
public static final String UAS_DATA_SUFFIX = "-UAS_DATA.tsv";
public void addUasData(
final double [][][] targets_single) {
boolean save_uas_tsv = true; // set to false to disable TSV file generation
int num_seq = targets_single.length;
UasLogReader uasLogReader = cuasMotion.getUasLogReader();
if (uasLogReader == null) {
System.out.println("addUasData(): no UAS log reader available - flight log circle will NOT be drawn");
return;
}
String [] slice_titles = cuasMotion.getSliceTitles(); // timestamps
int tilesX = cuasMotion.getTilesX();
int tilesY = targets_single[0].length / tilesX;
// Update camera reference LLA from center_CLT.did_ins_2 if available.
// quadCLT_main (the ImageJ plugin template) has no IMS loaded, so the fix in
// TwoQuadCLT cannot set it there; center_CLT is the right source.
if (center_CLT != null && center_CLT.hasIns()) {
double [] cameraLla = center_CLT.getLla();
if (cameraLla != null && (cameraLla[0] != 0.0 || cameraLla[1] != 0.0)) {
uasLogReader.setCameraLLA(cameraLla);
System.out.println("addUasData(): camera LLA set from center_CLT did_ins_2: lat="+
IJ.d2s(cameraLla[0],6)+" lon="+IJ.d2s(cameraLla[1],6)+" alt="+IJ.d2s(cameraLla[2],1));
}
} else {
System.out.println("addUasData(): WARNING center_CLT has no did_ins_2, NED will use UasLogReader default camera LLA");
}
double [] cam_atr = uasLogReader.getCameraATR();
System.out.println("addUasData(): camera ATR (azimuth/tilt/roll rad): az="+IJ.d2s(cam_atr[0],4)+
" tilt="+IJ.d2s(cam_atr[1],4)+" roll="+IJ.d2s(cam_atr[2],4)+
" (az_deg="+IJ.d2s(cam_atr[0]*180.0/Math.PI,2)+")");
double [] cam_lla = uasLogReader.getCameraLLA();
double uas_home_alt = uasLogReader.getUasHomeAlt();
double [] ned_corr = uasLogReader.getNedCorr();
System.out.println("addUasData(): camera LLA: lat="+IJ.d2s(cam_lla[0],6)+" lon="+IJ.d2s(cam_lla[1],6)+" alt="+IJ.d2s(cam_lla[2],1)+
" uas_home_alt="+IJ.d2s(uas_home_alt,3)+" (ref_alt="+IJ.d2s(-uas_home_alt,3)+")"+
" ned_corr=["+IJ.d2s(ned_corr[0],2)+","+IJ.d2s(ned_corr[1],2)+","+IJ.d2s(ned_corr[2],2)+"]");
String [] slice_titles = cuasMotion.getSliceTitles(); // timestamps
int tilesX = cuasMotion.getTilesX();
int tilesY = targets_single[0].length / tilesX;
System.out.println("addUasData(): UAS log reader present, checking "+num_seq+" sequences, tilesX="+tilesX+" tilesY="+tilesY);
StringBuilder tsv = save_uas_tsv ? new StringBuilder() : null;
if (tsv != null) {
tsv.append("seq\tts\tstatus\tpx\tpy\ttile_x\ttile_y\trange\tlat\tlon\talt\tnorth\teast\tdown\tcam_az\tcam_tilt\tcam_roll\n");
}
for (int nseq = 0; nseq < num_seq; nseq++) {
String timestamp = slice_titles[nseq];
double [] uas_pXpYDRange = uasLogReader.getUasPxPyDRange(timestamp); // px, py, d- cuas_infinity (true disparity), range
double [] llaned = uasLogReader.getUasLlaNed(timestamp); // lat, lon, alt, N, E, D
if (uas_pXpYDRange != null) {
double px = uas_pXpYDRange[0];
double py = uas_pXpYDRange[1];
......@@ -2727,6 +2761,9 @@ public class CuasRanging {
double range = uas_pXpYDRange[3];
int tileX = (int) (px/GPUTileProcessor.DTT_SIZE);
int tileY = (int) (py/GPUTileProcessor.DTT_SIZE);
String llaned_str = " lla=["+IJ.d2s(llaned[0],6)+","+IJ.d2s(llaned[1],6)+","+IJ.d2s(llaned[2],1)+
"] ned=["+IJ.d2s(llaned[3],1)+","+IJ.d2s(llaned[4],1)+","+IJ.d2s(llaned[5],1)+"]";
String status;
if ((tileX>=0) && (tileY>=0) && (tileX < tilesX) && (tileY < tilesY)) {
int ntile = tileX + tileY * tilesX;
if (targets_single[nseq][ntile] == null) {
......@@ -2738,9 +2775,38 @@ public class CuasRanging {
tile[CuasMotionLMA.RSLT_FL_PY] = py;
tile[CuasMotionLMA.RSLT_FL_DISP] = disparity;
tile[CuasMotionLMA.RSLT_FL_RANGE] = range;
status = "IN FoV";
System.out.println("addUasData(): seq="+nseq+" ts="+timestamp+
" UAS IN FoV: px="+IJ.d2s(px,1)+" py="+IJ.d2s(py,1)+
" tile=["+tileX+","+tileY+"] range="+IJ.d2s(range,1)+" disp="+IJ.d2s(disparity,4)+
llaned_str);
} else {
status = "OUT OF FoV";
System.out.println("addUasData(): seq="+nseq+" ts="+timestamp+
" UAS OUT OF FoV: px="+IJ.d2s(px,1)+" py="+IJ.d2s(py,1)+
" tile=["+tileX+","+tileY+"] (bounds 0.."+(tilesX-1)+", 0.."+(tilesY-1)+
") range="+IJ.d2s(range,1)+llaned_str);
}
if (tsv != null) {
tsv.append(nseq+"\t"+timestamp+"\t"+status+"\t"+
px+"\t"+py+"\t"+tileX+"\t"+tileY+"\t"+range+"\t"+
llaned[0]+"\t"+llaned[1]+"\t"+llaned[2]+"\t"+
llaned[3]+"\t"+llaned[4]+"\t"+llaned[5]+"\t"+
cam_atr[0]+"\t"+cam_atr[1]+"\t"+cam_atr[2]+"\n");
}
} else {
System.out.println("addUasData(): seq="+nseq+" ts="+timestamp+" no UAS log entry for this timestamp"+
(llaned != null ? " lla=["+IJ.d2s(llaned[0],6)+","+IJ.d2s(llaned[1],6)+","+IJ.d2s(llaned[2],1)+"]" : ""));
if (tsv != null) {
tsv.append(nseq+"\t"+timestamp+"\tno entry\t\t\t\t\t\t"+
(llaned != null ? llaned[0]+"\t"+llaned[1]+"\t"+llaned[2] : "\t\t")+
"\t\t\t\t"+cam_atr[0]+"\t"+cam_atr[1]+"\t"+cam_atr[2]+"\n");
}
}
}
}
if (tsv != null) {
center_CLT.saveStringInModelDirectory(tsv.toString(), UAS_DATA_SUFFIX, false);
}
}
// relies on calcMatchingTargetsLengths(.., true,...) called from recalcOmegas() to set [RSLT_GLOBAL]
......
......@@ -56,18 +56,34 @@ public class UasLogReader {
public void setCameraLLA(double [] camera_lla){
if (camera_lla != null) {
this.camera_lla = camera_lla.clone();
System.out.println("UasLogReader.setCameraLLA(): lat="+camera_lla[0]+" lon="+camera_lla[1]+" alt="+camera_lla[2]+
" (reference altitude used for NED: uas_home_alt="+uas_home_alt+" -> camera_lla0_alt="+(-uas_home_alt)+")");
}
}
public void setCameraATR(double [] atr) {
if (atr != null) {
this.camera_ATR = atr.clone();
System.out.println("UasLogReader.setCameraATR(): az="+atr[0]+" tilt="+atr[1]+" roll="+atr[2]+
" (az_deg="+(atr[0]*180.0/Math.PI)+")");
}
}
public double [] getCameraATR() {
return this.camera_ATR;
}
public double [] getCameraLLA() {
return this.camera_lla.clone();
}
public double getUasHomeAlt() {
return this.uas_home_alt;
}
public double [] getNedCorr() {
return this.ned_corr.clone();
}
public double getStartTimestamp() {
return start_timestamp;
......@@ -182,7 +198,7 @@ public class UasLogReader {
public double [] getNED(double timestamp) {
UasLogRecord uas_loc = interpolate(timestamp);
double [] camera_lla0 = {camera_lla[0], camera_lla[1], -uas_home_alt};
double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt};
double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt};
double [] ned = Imx5.nedFromLla (
uas_lla, // double [] lla,
camera_lla0); // double [] lla_ref)
......@@ -191,6 +207,17 @@ public class UasLogReader {
}
return ned;
}
/** Returns {lat, lon, alt, north, east, down} for the given scene timestamp string. */
public double [] getUasLlaNed(String stimestamp) {
double timestamp = Double.parseDouble(stimestamp.replace("_", "."));
UasLogRecord uas_loc = interpolate(timestamp);
double [] camera_lla0 = {camera_lla[0], camera_lla[1], -uas_home_alt};
double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt};
double [] ned = Imx5.nedFromLla(uas_lla, camera_lla0);
for (int i = 0; i < 3; i++) ned[i] += ned_corr[i];
return new double[] {uas_lla[0], uas_lla[1], uas_lla[2], ned[0], ned[1], ned[2]};
}
/*
>>> NED = [-1757.6742631032464, -581.1933833090495, -60.759999999999785]
......
......@@ -683,7 +683,38 @@ public class QuadCLTCPU {
e.printStackTrace();
}
}
// Load did_ins_2 from the IMS file saved by createCenterClt() (averaged LLA from sequence scenes).
// Only reads the did_ins_2 entry; does not call EventLogger.
if (center_CLT.did_ins_2 == null) {
// Mirror saveIms() for physical scenes: file is above the version subdir.
// center_CLT.getImagePath() == <center_dir>/v04, so getParent() == <center_dir>
String center_scene_dir = new File(center_CLT.getImagePath()).getParent();
String ims_path = center_scene_dir + Prefs.getFileSeparator() +
center_CLT.getImageName() + ref_clt.correctionsParameters.imsSuffix;
Properties ims_props = loadProperties(ims_path, null, true);
if (ims_props != null) {
Did_ins_2 center_did_ins_2 = new Did_ins_2("did_ins_2-", ims_props);
if (center_did_ins_2.lla != null &&
(center_did_ins_2.lla[0] != 0.0 || center_did_ins_2.lla[1] != 0.0)) {
center_CLT.did_ins_2 = center_did_ins_2;
if (debugLevel > -3) {
System.out.println("restoreCenterClt(): restored center_CLT did_ins_2 LLA: lat=" +
IJ.d2s(center_did_ins_2.lla[0], 6) +
" lon=" + IJ.d2s(center_did_ins_2.lla[1], 6) +
" alt=" + IJ.d2s(center_did_ins_2.lla[2], 1));
}
} else {
System.out.println("restoreCenterClt(): WARNING: IMS file has no valid did_ins_2 LLA for " +
center_CLT.getImageName() + " (file: " + ims_path + ")");
}
} else {
if (debugLevel > -3) {
System.out.println("restoreCenterClt(): no IMS file for center_CLT, did_ins_2 stays null (" + ims_path + ")");
}
}
}
return center_CLT;
}
......
......@@ -8535,6 +8535,9 @@ if (debugLevel > -100) return true; // temporarily !
uasLogReader = new UasLogReader(uas_log_path, quadCLT_main.correctionsParameters.cuasUasTimeStamp, null, quadCLT_main);
uasLogReader.setCameraATR(quadCLT_main.correctionsParameters.cuasCameraATR);
uasLogReader.setUASHomeNed(quadCLT_main.correctionsParameters.cuasSetHome? quadCLT_main.correctionsParameters.cuasUASHome : null);
// Camera LLA is NOT set here: quadCLT_main is the global template instance and
// has no IMS data loaded. setCameraLLA() is called in CuasRanging.addUasData()
// once center_CLT.did_ins_2 is available (from createCenterClt() or restoreCenterClt()).
}
switch (cuas_proc_mode) {
case 0 :
......
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