RealSense-D435i-G1/scripts/compare_host_align.py

"""Compare Intel's align on PC2 with an align on the host, on the SAME frameset. Read-only.

    python scripts/compare_host_align.py --url http://192.168.123.99:8878 [--framesets 10] [--tries 20]

For each frameset: GET /api/raw/latest (color + depth, not aligned) and
GET /api/capture (depth aligned on PC2) until both carry the same
frameset_seq; align the raw depth on the host with pyrealsense2 (a
software_device built from /api/intrinsics, then rs.align); report, pixel by
pixel, how the two aligned depths differ. Prints one JSON line per frameset
and a summary. Only GET requests; nothing on PC2 changes.

The host's pyrealsense2 may be newer than PC2's (2.58 here, 2.50 there): the
two aligns are then not even the same code version. It must have
software_video_frame.depth_units: pyrealsense2 2.50 has not, and there the
software frames' depth scale is 0, so rs.align returns an (almost) empty
depth WITHOUT any error. The script refuses to run on such a version.
"""

import argparse
import base64
import json
import sys

import cv2
import httpx
import numpy as np
import pyrealsense2 as rs

SYNC_WAIT_MS = 200     # wait for the syncer's frameset after a push
SYNC_PUSHES_MAX = 5    # the syncer may emit the first pair split (color alone): push the same pair again


# Calibration ------------------------------------------------------------------

def rs_intrinsics(stream: dict) -> rs.intrinsics:
    """rs.intrinsics from one stream of /api/intrinsics ("ir" or "rgb")."""
    d = stream["distortion"] or {"k1": 0, "k2": 0, "k3": 0, "p1": 0, "p2": 0}
    intr = rs.intrinsics()
    intr.width, intr.height = stream["width"], stream["height"]
    intr.fx, intr.fy, intr.ppx, intr.ppy = stream["fx"], stream["fy"], stream["cx"], stream["cy"]
    intr.model = getattr(rs.distortion, stream["model"] or "none")
    intr.coeffs = [d["k1"], d["k2"], d["p1"], d["p2"], d["k3"]]   # the SDK's order
    return intr


def rs_extrinsics(ext: dict) -> rs.extrinsics:
    """rs.extrinsics depth->color from the API's (rotation row-major, translation mm)."""
    r = ext["rotation"]
    out = rs.extrinsics()
    out.rotation = [r[0], r[3], r[6], r[1], r[4], r[7], r[2], r[5], r[8]]   # back to column-major
    out.translation = [t / 1000.0 for t in ext["translation"]]
    return out


# Host align ---------------------------------------------------------------------

def require_software_depth_units() -> None:
    """Raise unless this pyrealsense2 can give software depth frames their units (not 2.50)."""
    if not hasattr(rs.software_video_frame(), "depth_units"):
        raise RuntimeError(
            f"pyrealsense2 {getattr(rs, '__version__', '(no __version__, e.g. 2.50)')} has no "
            "software_video_frame.depth_units: the host align would come out empty. Use a newer one (2.58 works).")


class HostAligner:
    """rs.align(color) on frames pushed into a software_device with PC2's calibration."""

    def __init__(self, calibration: dict):
        require_software_depth_units()
        self._device = rs.software_device()
        self._depth = self._device.add_sensor("Depth")
        self._color = self._device.add_sensor("Color")
        self._depth.add_read_only_option(rs.option.depth_units, 0.001)   # we push millimeters
        self._depth_profile = self._depth.add_video_stream(
            _video_stream(rs.stream.depth, 0, rs.format.z16, 2, rs_intrinsics(calibration["ir"])))
        self._color_profile = self._color.add_video_stream(
            _video_stream(rs.stream.color, 1, rs.format.bgr8, 3, rs_intrinsics(calibration["rgb"])))
        self._depth_profile.register_extrinsics_to(self._color_profile, rs_extrinsics(calibration["extrinsics_ir_to_rgb"]))
        self._sync = rs.syncer()
        self._align = rs.align(rs.stream.color)
        self._number = 0
        self._started = []
        for sensor, profile in ((self._depth, self._depth_profile), (self._color, self._color_profile)):
            sensor.open(profile)
            sensor.start(self._sync)
            self._started.append(sensor)

    def close(self) -> None:
        for sensor in self._started:
            sensor.stop()
            sensor.close()
        self._started = []

    def __enter__(self):
        return self

    def __exit__(self, *exc):
        self.close()

    def align(self, depth_mm: np.ndarray, color_bgr: np.ndarray) -> np.ndarray:
        """Depth (mm, depth grid) aligned onto the color grid by rs.align, as uint16 mm."""
        depth_mm, color_bgr = np.ascontiguousarray(depth_mm), np.ascontiguousarray(color_bgr)
        for _ in range(SYNC_PUSHES_MAX):
            self._number += 1
            stamp_ms = self._number * 1000.0
            _push(self._depth, self._depth_profile, depth_mm, 2, self._number, stamp_ms, depth=True)
            _push(self._color, self._color_profile, color_bgr, 3, self._number, stamp_ms)
            frames = self._complete_frameset(self._number)
            if frames is not None:
                aligned = self._align.process(frames)
                return np.asanyarray(aligned.get_depth_frame().get_data()).copy()
        raise RuntimeError(f"syncer gave no complete frameset in {SYNC_PUSHES_MAX} pushes")

    def _complete_frameset(self, number: int):
        """The syncer's frameset holding depth AND color of frame `number`, or None."""
        for _ in range(2):   # a split pair comes out as two framesets
            try:
                frames = self._sync.wait_for_frames(SYNC_WAIT_MS)
            except RuntimeError:   # "Frame did not arrive in time": try pushing again
                return None
            depth = frames.get_depth_frame()
            if frames.size() == 2 and depth and depth.get_frame_number() == number:
                return frames
        return None


def _video_stream(kind, uid: int, fmt, bpp: int, intrinsics: rs.intrinsics) -> rs.video_stream:
    vs = rs.video_stream()
    vs.type, vs.index, vs.uid, vs.fmt, vs.bpp = kind, 0, uid, fmt, bpp
    vs.width, vs.height, vs.fps, vs.intrinsics = intrinsics.width, intrinsics.height, 30, intrinsics
    return vs


def _push(sensor, profile, pixels: np.ndarray, bpp: int, number: int, stamp_ms: float, depth: bool = False) -> None:
    frame = rs.software_video_frame()
    frame.pixels = pixels
    frame.bpp = bpp
    frame.stride = pixels.shape[1] * bpp
    frame.timestamp = stamp_ms
    frame.domain = rs.timestamp_domain.hardware_clock
    frame.frame_number = number
    frame.profile = profile.as_video_stream_profile()
    if depth:
        frame.depth_units = 0.001   # millimeters (checked to exist by require_software_depth_units)
    sensor.on_video_frame(frame)


# PC2 side (GET only) ------------------------------------------------------------

def _data(response: httpx.Response) -> dict:
    response.raise_for_status()
    body = response.json()
    if not body.get("success"):
        raise RuntimeError(f"{response.request.url}: {body.get('error')}")
    return body["data"]


def _decode(b64: str, flags: int) -> np.ndarray:
    return cv2.imdecode(np.frombuffer(base64.b64decode(b64), np.uint8), flags)


def fetch_same_frameset(client: httpx.Client, tries: int) -> tuple[dict, dict, int]:
    """(raw, capture, attempts): /api/raw/latest and /api/capture of one frameset_seq."""
    for attempt in range(1, tries + 1):
        raw = _data(client.get("/api/raw/latest"))
        capture = _data(client.get("/api/capture"))
        if capture.get("frameset_seq") == raw["frameset_seq"]:
            return raw, capture, attempt
    raise RuntimeError(f"no matching frameset in {tries} attempts (another client may keep the align busy)")


# Comparison (pure) -------------------------------------------------------------

def compare_depths(pc2: np.ndarray, host: np.ndarray) -> dict:
    """Pixel-by-pixel difference of two aligned depth maps in mm (same shape)."""
    if pc2.shape != host.shape:
        raise ValueError(f"shapes differ: PC2 {pc2.shape}, host {host.shape}")
    both = (pc2 > 0) & (host > 0)
    diff = np.abs(pc2[both].astype(np.int32) - host[both].astype(np.int32))
    total = pc2.size
    return {
        "pixels": int(total),
        "valid_both": int(both.sum()),
        "only_pc2": int(((pc2 > 0) & (host == 0)).sum()),
        "only_host": int(((host > 0) & (pc2 == 0)).sum()),
        "equal_fraction_of_valid_both": float((diff == 0).mean()) if diff.size else None,
        "abs_diff_mm": None if not diff.size else {
            "mean": float(diff.mean()), "median": float(np.median(diff)),
            "p99": float(np.percentile(diff, 99)), "max": int(diff.max()),
        },
    }


def compare_one(client: httpx.Client, aligner: HostAligner, tries: int) -> dict:
    raw, capture, attempts = fetch_same_frameset(client, tries)
    host = aligner.align(_decode(raw["depth_png_b64"], cv2.IMREAD_UNCHANGED),
                         _decode(raw["color_jpeg_b64"], cv2.IMREAD_COLOR))
    pc2 = _decode(capture["depth_png_b64"], cv2.IMREAD_UNCHANGED)
    return {"frameset_seq": raw["frameset_seq"], "attempts": attempts, **compare_depths(pc2, host)}


def main(argv=None) -> int:
    ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
    ap.add_argument("--url", required=True, help="RealSense service, e.g. http://192.168.123.99:8878")
    ap.add_argument("--framesets", type=int, default=10)
    ap.add_argument("--tries", type=int, default=20, help="attempts to get raw and aligned of one frameset")
    ap.add_argument("--api-key", default="")
    args = ap.parse_args(argv)
    headers = {"X-Api-Key": args.api_key} if args.api_key else {}
    results = []
    with httpx.Client(base_url=args.url, headers=headers, timeout=10) as client:
        calibration = _data(client.get("/api/intrinsics"))
        with HostAligner(calibration) as aligner:
            for _ in range(args.framesets):
                result = compare_one(client, aligner, args.tries)
                results.append(result)
                print(json.dumps(result))
    means = [r["abs_diff_mm"]["mean"] for r in results if r["abs_diff_mm"]]
    print(json.dumps({"summary": {"framesets": len(results), "pyrealsense2_host": getattr(rs, "__version__", "unknown"),
                                  "mean_abs_diff_mm_per_frameset": means}}))
    return 0


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