Commit 9942ae46 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

CLAUDE: fopen_turn_finder.py — add lat/lon from *-ims.corr-xml 



*-ims.corr-xml in the scene root dir always has an INS-fused GPS fix
(did_ins_2-lla, fallback did_gps1_pos-lla) giving the representative
position of the sequence for map location.

Changes:
- find_xml_files: also yields ims_path (*-ims.corr-xml from scene root)
- parse_lla(): reads lat/lon from INS or GPS1 LLA entry
- lat/lon added to metrics, console tables, and TSV output

Co-authored-by: default avatarClaude <claude@elphel.com>
parent 2b1e81b3

scripts/fopen_turn_finder.py

+59 −12
Original line number Diff line number Diff line
@@ -197,12 +197,13 @@ def sequence_metrics(pose_arr, dt_arr): 


def find_xml_files(root):

    """

    Yield (scene_ts_str, xml_path, hist_path_or_None) for every scene dir

    under root.  Picks the latest version subdirectory by name (e.g. v88 > v80).

    Yield (scene_ts_str, xml_path, hist_path_or_None, ims_path_or_None)

    for every scene dir under root.

    Picks the latest version subdirectory by name (e.g. v88 > v80).

    Skips directories whose name ends with '-index'.



    hist_path is the *-elevations_histogram.csv inside the version subdir,

    or None if not present.

    hist_path  — *-elevations_histogram.csv inside the version subdir (AGL)

    ims_path   — *-ims.corr-xml in the scene root dir (GPS lat/lon/alt)

    """

    try:

        scene_dirs = sorted(os.listdir(root))
@@ -231,8 +232,11 @@ def find_xml_files(root): 
        ver_path  = os.path.join(scene_path, ver)

        xml_path  = os.path.join(ver_path,   f"{scene}-INTERFRAME.corr-xml")

        hist_path = os.path.join(ver_path,   f"{scene}-elevations_histogram.csv")

        ims_path  = os.path.join(scene_path, f"{scene}-ims.corr-xml")

        if os.path.isfile(xml_path):

            yield scene, xml_path, (hist_path if os.path.isfile(hist_path) else None)

            yield (scene, xml_path,

                   hist_path if os.path.isfile(hist_path) else None,

                   ims_path  if os.path.isfile(ims_path)  else None)





def parse_agl(hist_path):
@@ -256,6 +260,41 @@ def parse_agl(hist_path): 
        return float('nan')





def parse_lla(ims_path):

    """

    Read latitude, longitude, altitude from *-ims.corr-xml.



    The file lives in the scene root directory (not the version subdir)

    and always contains a single representative GPS fix for the sequence.



    Key priority (most to least accurate):

      did_ins_2-lla   — INS fused GPS+IMU position  (preferred)

      did_gps1_pos-lla — raw GPS1 position           (fallback)



    Format of each entry value: "lat, lon, alt_m"



    Returns (lat, lon) as floats, or (nan, nan) on failure.

    """

    nan = float('nan')

    try:

        tree    = ET.parse(ims_path)

        entries = {e.get("key"): e.text

                   for e in tree.getroot().findall("entry")}

    except Exception:

        return nan, nan



    for key in ("did_ins_2-lla", "did_gps1_pos-lla"):

        val = entries.get(key, "")

        if val:

            try:

                parts = [float(v) for v in val.split(",")]

                if len(parts) >= 2:

                    return parts[0], parts[1]

            except ValueError:

                continue

    return nan, nan





# ─────────────────────────────── main ────────────────────────────────────── #



def main():
@@ -280,7 +319,7 @@ def main(): 


    print(f"Scanning {args.root} …", flush=True)

    rows = []

    for scene_ts, xml_path, hist_path in find_xml_files(args.root):

    for scene_ts, xml_path, hist_path, ims_path in find_xml_files(args.root):

        pose_arr, dt_arr = parse_xml(xml_path)

        if pose_arr is None:

            continue
@@ -288,6 +327,9 @@ def main(): 
        m['scene_ts'] = scene_ts

        m['xml_path'] = xml_path

        m['agl_m']    = parse_agl(hist_path) if hist_path else float('nan')

        lat, lon      = parse_lla(ims_path)  if ims_path  else (float('nan'), float('nan'))

        m['lat']      = lat

        m['lon']      = lon

        rows.append(m)



    print(f"Parsed {len(rows)} sequences.", flush=True)
@@ -321,8 +363,8 @@ def main(): 
    display.sort(key=lambda r: r.get(sort_key) or 0, reverse=True)



    # ── console report ────────────────────────────────────────────────────

    hdr = (f"  {'Scene':>22}  {'fr':>4}  {'AGL(m)':>7}  {'range(°)':>8}  "

           f"{'dt_max(°/s)':>11}  {'at timestamp':>20}  "

    hdr = (f"  {'Scene':>22}  {'fr':>4}  {'AGL(m)':>7}  {'lat':>11}  {'lon':>12}  "

           f"{'range(°)':>8}  {'dt_max(°/s)':>11}  {'at timestamp':>20}  "

           f"{'glitch':>7}  {'fd_max(°/s)':>11}")



    for axis, label in (('r', 'R (heading/course)'),
@@ -338,8 +380,13 @@ def main(): 
        print(hdr)

        for r in ranked[:args.top]:

            agl  = r.get('agl_m', float('nan'))

            lat  = r.get('lat',   float('nan'))

            lon  = r.get('lon',   float('nan'))

            agl_s = f"{agl:7.1f}" if np.isfinite(agl) else "    n/a"

            lat_s = f"{lat:11.6f}" if np.isfinite(lat) else "        n/a"

            lon_s = f"{lon:12.6f}" if np.isfinite(lon) else "         n/a"

            print(f"  {r['scene_ts']:>22}  {r['n_frames']:>4}  {agl_s}  "

                  f"{lat_s}  {lon_s}  "

                  f"{r[f'{axis}_range(deg)']:>8.2f}  "

                  f"{r[f'{axis}_dt_max(deg/s)']:>11.3f}  "

                  f"{r[f'{axis}_dt_ts']:>20.3f}  "
@@ -349,7 +396,7 @@ def main(): 
    # ── TSV output ────────────────────────────────────────────────────────

    if args.out:

        fieldnames = [

            'scene_ts', 'n_frames', 'duration_s', 'agl_m',

            'scene_ts', 'n_frames', 'duration_s', 'agl_m', 'lat', 'lon',

            'r_dt_max(deg/s)', 'r_dt_ts', 'r_range(deg)', 'r_glitch', 'r_fd_max(deg/s)',

            't_dt_max(deg/s)', 't_dt_ts', 't_range(deg)', 't_glitch', 't_fd_max(deg/s)',

            'a_dt_max(deg/s)', 'a_dt_ts', 'a_range(deg)', 'a_glitch', 'a_fd_max(deg/s)',