feat(pointcloud): support foundation stereo depth pipeline and global cloud integration

tests/test_foundation_stereo_trt.py

@@ -0,0 +1,114 @@
+import asyncio
+import os
+import sys
+
+import cv2
+import numpy as np
+
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
+
+from tool.foundation_stereo_trt import FoundationStereoTRT
+
+
+def _load_calib(calib_path: str):
+    with open(calib_path, "r") as f:
+        lines = [ln.strip() for ln in f.readlines() if ln.strip()]
+
+    if len(lines) < 5:
+        raise RuntimeError(f"Unexpected calib file format in {calib_path}")
+
+    k_rows = []
+    for i in range(1, 4):
+        k_rows.append([float(v) for v in lines[i].split()])
+    k = np.array(k_rows, dtype=np.float32)
+
+    baseline_line = lines[-1]
+    _, val_str = baseline_line.split(":", 1)
+    baseline = float(val_str.strip())
+    return k, baseline
+
+
+def _save_visuals(data_dir: str, disp: np.ndarray, depth: np.ndarray):
+    np.save(os.path.join(data_dir, "foundation_disp.npy"), disp)
+    np.save(os.path.join(data_dir, "foundation_depth.npy"), depth)
+
+    if np.isfinite(disp).any():
+        dmin = np.nanmin(disp[np.isfinite(disp)])
+        dmax = np.nanmax(disp[np.isfinite(disp)])
+        if dmax > dmin:
+            disp_norm = (disp - dmin) / (dmax - dmin)
+        else:
+            disp_norm = np.zeros_like(disp, dtype=np.float32)
+    else:
+        disp_norm = np.zeros_like(disp, dtype=np.float32)
+    disp_u8 = np.clip(disp_norm * 255.0, 0, 255).astype(np.uint8)
+    cv2.imwrite(
+        os.path.join(data_dir, "foundation_disp_vis.png"),
+        cv2.applyColorMap(disp_u8, cv2.COLORMAP_TURBO),
+    )
+
+    valid = np.isfinite(depth) & (depth > 0)
+    if valid.any():
+        zmin = np.nanmin(depth[valid])
+        zmax = np.nanmax(depth[valid])
+        if zmax > zmin:
+            depth_norm = (np.clip(depth, zmin, zmax) - zmin) / (zmax - zmin)
+        else:
+            depth_norm = np.zeros_like(depth, dtype=np.float32)
+    else:
+        depth_norm = np.zeros_like(depth, dtype=np.float32)
+    depth_u8 = np.clip(depth_norm * 255.0, 0, 255).astype(np.uint8)
+    cv2.imwrite(
+        os.path.join(data_dir, "foundation_depth_vis.png"),
+        cv2.applyColorMap(depth_u8, cv2.COLORMAP_VIRIDIS),
+    )
+
+
+def _run_foundation_case(data_dir: str):
+    left_path = os.path.join(data_dir, "left.png")
+    right_path = os.path.join(data_dir, "right.png")
+    calib_path = os.path.join(data_dir, "calib.txt")
+
+    assert os.path.exists(left_path), f"Missing left image: {left_path}"
+    assert os.path.exists(right_path), f"Missing right image: {right_path}"
+    assert os.path.exists(calib_path), f"Missing calib file: {calib_path}"
+
+    left = cv2.imread(left_path, cv2.IMREAD_GRAYSCALE)
+    right = cv2.imread(right_path, cv2.IMREAD_GRAYSCALE)
+    assert left is not None and right is not None, "Failed to read stereo images"
+    assert left.shape == right.shape, f"Left/right shapes mismatch: {left.shape} vs {right.shape}"
+
+    k, baseline = _load_calib(calib_path)
+    fx = float(k[0, 0])
+
+    stereo_engine = FoundationStereoTRT()
+    disp, depth = asyncio.run(
+        stereo_engine.infer(
+            left,
+            right,
+            np.array([[baseline]], dtype=np.float32),
+            np.array([[fx]], dtype=np.float32),
+        )
+    )
+
+    assert disp.shape == left.shape, f"Disparity shape mismatch: {disp.shape} vs {left.shape}"
+    assert depth.shape == left.shape, f"Depth shape mismatch: {depth.shape} vs {left.shape}"
+    assert np.isfinite(disp).any(), "No finite disparity values"
+    assert np.isfinite(depth).any(), "No finite depth values"
+
+    _save_visuals(data_dir, disp, depth)
+
+
+def test_foundation_stereo_trt_with_looper_data():
+    _run_foundation_case("/tinynav/tests/data/looper")
+
+
+def test_foundation_stereo_trt_with_realsense_data():
+    _run_foundation_case("/tinynav/tests/data/realsense")
+
+
+if __name__ == "__main__":
+    test_foundation_stereo_trt_with_looper_data()
+    print("FoundationStereo TRT looper test passed.")
+    test_foundation_stereo_trt_with_realsense_data()
+    print("FoundationStereo TRT realsense test passed.")

tinynav/models/Makefile

@@ -13,6 +13,7 @@ DINO_ONNX        := dinov2_base_224x224_fp16.onnx
 SUPERPOINT_ONNX  := superpoint_fp16_dynamic.onnx
 LIGHTGLUE_ONNX   := lightglue_fp16.onnx
 RETINIFY_ONNX    := retinify_0_1_5_dynamic.onnx
+FOUNDATION_STEREO_ONNX := foundation_stereo.onnx
 
 
 DINO_ENGINE       := $(basename $(DINO_ONNX))_$(ARCH).plan
@@ -23,17 +24,19 @@ LIGHTGLUE_ENGINE  := $(basename $(LIGHTGLUE_ONNX))_$(ARCH).plan
 # - Retinify: one profile, min/max shapes for RealSense (480x848) and Looper (640x544).
 SUPERPOINT_ENGINE := $(basename $(SUPERPOINT_ONNX))_$(ARCH).plan
 RETINIFY_ENGINE   := $(basename $(RETINIFY_ONNX))_$(ARCH).plan
+FOUNDATION_STEREO_ENGINE := $(basename $(FOUNDATION_STEREO_ONNX))_$(ARCH).plan
 
 ENGINES := $(DINO_ENGINE) $(LIGHTGLUE_ENGINE) $(SUPERPOINT_ENGINE) $(RETINIFY_ENGINE)
 
-.PHONY: all dinov2 superpoint lightglue retinify clean help
+.PHONY: all dinov2 superpoint lightglue retinify foundation_stereo clean help
 
 all: $(ENGINES)
 
 dinov2: $(DINO_ENGINE)
 superpoint: $(SUPERPOINT_ENGINE)
 lightglue: $(LIGHTGLUE_ENGINE)
 retinify: $(RETINIFY_ENGINE)
+foundation_stereo: $(FOUNDATION_STEREO_ENGINE)
 
 help:
     @echo "Targets:"
@@ -42,6 +45,7 @@ help:
 	@echo "  make superpoint   # Build SuperPoint"
 	@echo "  make lightglue    # Build LightGlue"
 	@echo "  make retinify     # Build Retinify (dynamic, RealSense+Looper)"
+	@echo "  make foundation_stereo # Build foundation stereo"
 	@echo "  make clean        # Remove *_$(ARCH).plan"
 
 $(DINO_ENGINE): $(DINO_ONNX)
@@ -73,6 +77,12 @@ $(RETINIFY_ENGINE): $(RETINIFY_ONNX)
         --layerPrecisions='/cost_volume/Unsqueeze:fp16,/cost_volume/post_process/conv_block3/conv_block3.0/BatchNormalization:fp32,/cost_volume/post_process/norm/norm.0/BatchNormalization:fp32,/disparity_refinement/disp_block3/disp_block3.2/block/block.0/BatchNormalization:fp32,/disparity_refinement/disp_block3/disp_block3.2/block/block.1/Relu:fp32,/disparity_refinement/disp_block5/disp_block5.9/bn/block/block.0/BatchNormalization:fp32,/disparity_refinement/disp_block5/disp_block5.9/bn/block/block.1/Relu:fp32,/disparity_refinement/disp_block5/disp_block5.10/block/block.0/BatchNormalization:fp32,/disparity_refinement/disp_block5/disp_block5.10/block/block.1/Relu:fp32,/disparity_refinement/upsample2_conv/upsample2_conv.0/BatchNormalization:fp32' \
         --fp16
 
+# foundation_stereo.onnx export settings:
+#   --valid_iters 8
+#   --max_disp 192
+$(FOUNDATION_STEREO_ENGINE): $(FOUNDATION_STEREO_ONNX)
+	$(TRTEXEC) --onnx=$< --saveEngine=$@ --fp16
+
 
 clean:
 	rm -f *_$(ARCH).plan