orisys-sdk/examples/finger_pipeline.py

"""ORY-FINGER: UV-unrolled finger tactile pipeline.

Same shape as ``basic_pipeline.py`` - one object, one call per frame - with the finger-specific
capture path underneath:

    FingerRuntime  ->  runtime.pipeline()  ->  FrameSnapshot per frame

``FingerRuntime`` owns the camera, the Blender export (mesh, intrinsics, world-to-camera), the UV
unroll map and the valid-region mask. ``runtime.pipeline()`` runs those stages concurrently and
returns one immutable snapshot per frame, so there is nothing to sequence and no tuple to unpack.

Run:

    python examples/finger_pipeline.py --camera_id 0 --config finger
    python examples/finger_pipeline.py --video path/to/clip.mp4
    python examples/finger_pipeline.py --no-show          # headless: print only
    python examples/finger_pipeline.py --config path/to/sensor.json --video clip.mp4 --cal-path model.json --zero

Geometry comes from the selected bundle's manifest.json assets_dir; its UV cache follows the
geometry directory. Schema 3 requires matching processing config/geometry and a session zero.
Use --zero only when the first 30 frames after warmup are known to be unloaded. Without a zero,
schema 3 reports zero_unavailable. Use --help-advanced for SN selection and diagnostic overrides.
Keys: 'q' quit, 'r' reset the tracker and clear the schema 3 session reference.
"""

import argparse
import math
import os
import statistics
import sys
from pathlib import Path

os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0")

import cv2
from _display import draw_flow_arrows

from orisys.configs import resolve_sensor_bundle
from orisys.finger import FingerRuntime


def configured_export_dir(config):
    """Reuse the selected bundle's geometry, including an explicit sensor.json path."""
    path = Path(config)
    bundle = resolve_sensor_bundle(str(path.resolve().parent) if path.is_file() else config)
    if bundle is None:
        return None
    if path.is_file() and bundle.sensor_json != path.resolve():
        return None
    if bundle.manifest_json is not None and bundle.assets_dir is None:
        raise ValueError(f"No usable assets_dir in {bundle.manifest_json}; use --export-dir")
    return str(bundle.assets_dir) if bundle.assets_dir is not None else None


def measure_unloaded_reference(runtime):
    """Measure before pipeline threads start; --zero asserts these frames are unloaded."""
    tactile = runtime.tactile
    model = tactile.raw_sensor.calibration
    if getattr(model, "schema_version", None) != 3:
        raise ValueError("--zero requires a loaded schema 3 calibration")
    info = tactile.info
    axes = [(axis, key) for axis, key in
            (("fn", info.FNORMAL_RAW), ("fx", info.FSHEARX_RAW), ("fy", info.FSHEARY_RAW))
            if axis in model.channels]
    samples = {axis: [] for axis, _ in axes}
    print("Measuring session zero from 30 unloaded frames...")
    for _ in range(30):
        ok, _ = runtime.grab_and_process(color=False)
        if not ok:
            raise ValueError("Input ended before 30 unloaded frames; session zero was not set")
        tactile.compute_deformation()
        values = tactile.read_info(*(key for _, key in axes))
        if not all(math.isfinite(value) for value in values):
            raise ValueError("Non-finite unloaded reading; session zero was not set")
        for (axis, _), value in zip(axes, values):
            samples[axis].append(value)
    runtime.set_calibration_reference(
        {axis: statistics.median(values) for axis, values in samples.items()},
        reference_id="startup-unloaded-reference")


def main() -> int:
    advanced = "--help-advanced" in sys.argv
    parser = argparse.ArgumentParser(description="UV-unrolled finger tactile pipeline",
                                     epilog="Use --help-advanced for SN selection and diagnostic overrides.")
    parser.add_argument("--help-advanced", action="help", help=argparse.SUPPRESS)
    def advanced_help(message):
        return message if advanced else argparse.SUPPRESS
    parser.add_argument("--camera_id", type=int, default=0, help="camera index (default 0)")
    parser.add_argument("-v", "--video", help="video file path; overrides --camera_id")
    parser.add_argument("-c", "--config", default="finger",
                        help="product config name or path (default: finger)")
    calibration = parser.add_mutually_exclusive_group()
    calibration.add_argument("--cal-path", "--cal_path", help="explicit calibration JSON (schema 1/2/3)")
    calibration.add_argument("--calibration-dir", "--calibration_dir",
                             help=advanced_help("select <directory>/<sensor_type>/<device_id>.json"))
    parser.add_argument("--device-id", "--device_id", help=advanced_help("actual device SN to validate"))
    zero = parser.add_mutually_exclusive_group()
    zero.add_argument("--zero", action="store_true",
                      help="measure session zero; first 30 frames after warmup MUST be unloaded")
    zero.add_argument("--zero-fn-raw", type=float,
                      help=advanced_help("measured unloaded FN raw for this session (schema 3 only)"))
    parser.add_argument("--export-dir", help=advanced_help("override bundle geometry export directory"))
    parser.add_argument("--map-cache", help=advanced_help("override UV map base path; SDK appends scale suffix"))
    parser.add_argument("--no-show", dest="show", action="store_false",
                        help="do not open the display window")
    parser.add_argument("-verbose", action="store_true", help="SDK verbose logs")
    args = parser.parse_args()
    if args.device_id and not (args.cal_path or args.calibration_dir):
        parser.error("--device-id requires --cal-path or --calibration-dir")
    if args.calibration_dir and not args.device_id:
        parser.error("--calibration-dir requires --device-id")
    if args.zero_fn_raw is not None and not math.isfinite(args.zero_fn_raw):
        parser.error("--zero-fn-raw must be finite")

    source = args.video if args.video else args.camera_id

    # ------------------------------------------------------------------ create the object
    options = {key: getattr(args, key) for key in
               ("cal_path", "calibration_dir", "device_id", "export_dir", "map_cache")
               if getattr(args, key) is not None}
    try:
        if "export_dir" not in options:
            export_dir = configured_export_dir(args.config)
            if export_dir is not None:
                options["export_dir"] = export_dir
        runtime = FingerRuntime.open(config=args.config, video=source, verbose=args.verbose, **options)
    except (RuntimeError, ValueError, OSError) as exc:
        print(str(exc), file=sys.stderr)
        return 1

    if args.verbose:
        g = runtime.geometry
        print(f"UV geometry: native={g.native_width}x{g.native_height} "
              f"source={g.source_width}x{g.source_height} "
              f"output={g.out_width}x{g.out_height}")

    if args.show:
        print("Keys: 'q' quit, 'r' reset tracker and clear session zero.")

    include = ("image", "flow") if args.show else ("image",)
    try:
        # Warmup may reset the session; apply a supplied reference afterwards and before threads start.
        runtime.warmup(color=False)
        if getattr(runtime.tactile.raw_sensor.calibration, "schema_version", None) == 3 and not args.device_id:
            print("Calibration file selected; hardware SN has not been independently verified.")
        if args.zero:
            measure_unloaded_reference(runtime)
        if args.zero_fn_raw is not None:
            model = runtime.tactile.raw_sensor.calibration
            if getattr(model, "schema_version", None) != 3:
                raise ValueError("--zero-fn-raw requires a loaded schema 3 calibration")
            runtime.set_calibration_reference({"fn": args.zero_fn_raw}, reference_id="cli-unloaded-reference")
        with runtime.pipeline(contact=True, include=include) as pipe:
            while True:
                # ------------------------------------------------------ read one frame
                snap = pipe.wait()
                if snap is None:
                    print("\nInput ended.")
                    break

                # ------------------------------------------------------ use the results
                coverage = runtime.tactile.mask_stats.get("coverage")
                coverage_text = f"{coverage:.2f}" if isinstance(coverage, float) else "n/a"
                fn_unit, fx_unit, fy_unit = snap.force_units or runtime.tactile.force_units
                status = ",".join(f"{axis}:{value}" for axis, value in (snap.calibration_status or {}).items())
                print(
                    f"FPS={snap.fps:5.1f}  "
                    f"normal={snap.normal_force:8.4f} {fn_unit}  "
                    f"shear=({snap.shear_x:7.4f} {fx_unit}, {snap.shear_y:7.4f} {fy_unit})  "
                    f"mask={coverage_text}  age={snap.age_ms:4.1f} ms  calibration={status or 'legacy/raw'}",
                    end="\r",
                )

                if args.show and snap.image is not None and snap.flow is not None:
                    cv2.imshow("tactile (UV unrolled)", draw_flow_arrows(
                        snap.image, snap.flow, threshold=0.4, grid_spacing=10,
                        valid_mask=runtime.tactile.valid_mask))

                key = cv2.waitKey(1) & 0xFF if args.show else -1
                if key == ord("q"):
                    print("\nExiting.")
                    break
                if key == ord("r"):
                    pipe.reset()
                    print("\nTracker reset. Schema 3 session zero cleared; a new unloaded reference is required.")
    except (RuntimeError, ValueError, OSError) as exc:
        print(str(exc), file=sys.stderr)
        return 1
    except KeyboardInterrupt:
        print("\nInterrupted.")
    finally:
        runtime.close()
        if args.show:
            cv2.destroyAllWindows()
    return 0


if __name__ == "__main__":
    sys.exit(main())