diff --git a/.gitignore b/.gitignore index 3dfdefc..bc86d31 100644 --- a/.gitignore +++ b/.gitignore @@ -6,9 +6,17 @@ __pycache__/ *.pyc .venv/ +# test-only reference-oracle python env (scripts/oracle.sh setup) +/.oracle-venv/ # Go build artifacts *.test *.out # recorder intermediate outputs /out/ + +# demo server: compiled binary + runtime data + model fetch log +/server/da3-server +/.cache/ +/scripts/.fetch.log +/server/server diff --git a/CMakeLists.txt b/CMakeLists.txt index 732bfd5..d8eda1a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,12 @@ set(DA_SOURCES src/colmap_export.cpp src/cam_pose.cpp src/linalg.cpp + src/sim3.cpp + src/stream.cpp + src/fuse.cpp + src/tsdf.cpp + src/icp.cpp + src/posegraph.cpp src/ray_pose.cpp src/nested.cpp src/depth_export.cpp @@ -92,6 +98,18 @@ if(DA_HAS_AVX512F) endif() target_link_libraries(depthanything PUBLIC ggml) +# OpenMP for the single-threaded CPU geometry post-processing (surface fusion, +# per-seam ICP, normal estimation). ggml already pulls OpenMP into the toolchain; +# link it into OUR target too so the #pragma omp loops in fuse.cpp/icp.cpp use all +# cores instead of one. Optional: without it the pragmas compile to no-ops. +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + target_link_libraries(depthanything PUBLIC OpenMP::OpenMP_CXX) + message(STATUS "depth-anything.cpp: OpenMP enabled for CPU geometry") +else() + message(STATUS "depth-anything.cpp: OpenMP NOT found — CPU geometry stays single-threaded") +endif() + if(DA_BUILD_CLI AND EXISTS ${DA_ROOT}/examples/cli/CMakeLists.txt) add_subdirectory(examples/cli) endif() diff --git a/include/da_capi.h b/include/da_capi.h index 8bb9a9c..862cdf3 100644 --- a/include/da_capi.h +++ b/include/da_capi.h @@ -6,8 +6,25 @@ extern "C" { #endif typedef struct da_ctx da_ctx; /* ABI version. 3: added da_capi_depth_dense, da_capi_points, da_capi_free_bytes. - 4: added da_capi_load_nested (two-branch metric model). */ + 4: added da_capi_load_nested (two-branch metric model). + 5: added da_capi_points_multi (fused multi-view cloud) + da_capi_gaussians. + 6: added da_capi_points_stream (sliding-window streaming cloud); removed the + unused da_capi_depth_pose_multi. + 7: da_capi_gaussians also returns the input camera intrinsics + processed size + (out_intr[9], out_w, out_h) so the viewer can render from the input view. + 8: da_capi_points_stream gained a `metric` flag (rescale the streamed cloud to + metres via the nested metric branch; requires a da_capi_load_nested ctx). + 9: added da_capi_stream_last_poses — per-input-frame camera poses from the last + da_capi_points_stream call (for the viewer flythrough). + 10: added da_capi_set_fuse_params — scene-relative TSDF voxel/truncation knobs for + the next streamed fuse (exposed as viewer sliders). */ int da_capi_abi_version(void); +/* Set scene-relative TSDF surface-fusion knobs consumed by the NEXT + da_capi_points_stream whose fuse flag is set. voxel_frac: voxel edge as a fraction + of the reconstruction's bbox diagonal (fusion detail; <=0 => 0.004). trunc_mult: + truncation as a multiple of the voxel edge (merge range = 2*trunc_mult voxels; + <=0 => 4). Split out because da_capi_points_stream is at the purego arg ceiling. */ +void da_capi_set_fuse_params(da_ctx* ctx, double voxel_frac, double trunc_mult); da_ctx* da_capi_load(const char* gguf_path, int n_threads); /* NULL on failure */ /* Load a NESTED metric model from its two branches: the anyview (GIANT) GGUF and the metric (ViT-L + DPT/sky) GGUF. The returned ctx runs the nested metric @@ -26,12 +43,6 @@ float* da_capi_depth_path(da_ctx* ctx, const char* image_path, int* out_h, int* void da_capi_free_floats(float* p); /* Run pose; fills ext[12] (3x4 row-major) and intr[9] (3x3). Returns 0 ok, -1 error. */ int da_capi_pose_path(da_ctx* ctx, const char* image_path, float out_ext[12], float out_intr[9]); -/* Multi-view depth+pose. n_images paths. Fills, per view i: out_ext[i*12], out_intr[i*9]. - Returns a malloc'd float[n*H*W] depth (view-major), sets *out_h,*out_w,*out_n; NULL on error. - Caller frees the returned buffer via da_capi_free_floats. */ -float* da_capi_depth_pose_multi(da_ctx* ctx, const char** image_paths, int n_images, - int* out_h, int* out_w, int* out_n, - float* out_ext /* n*12 */, float* out_intr /* n*9 */); /* Single-image 3D export. Runs the native depth+pose pipeline, captures the processed-resolution RGB colors, and writes a glTF-2.0 binary point cloud to out_glb. Returns 0 ok, -1 error (see da_capi_last_error). */ @@ -66,6 +77,76 @@ int da_capi_points(da_ctx* ctx, const char* image_path, float conf_thresh, int* out_n, float** out_xyz, unsigned char** out_rgb); /* Free a uint8 buffer returned by da_capi_points (out_rgb). */ void da_capi_free_bytes(unsigned char* p); + +/* Multi-view FUSED colored point cloud (DualDPT/pose-capable DA3 models; returns + -1 for mono/DA2). Runs ONE cross-view depth+pose pass over n_images and back- + projects every view into a single shared world frame, so the cloud is coherent + across frames (no per-frame scale drift). Recovers per-pixel processed RGB and a + perspective-correct per-point radius (~depth/focal * point_size). Keeps pixels + with confidence >= percentile(conf, conf_pct) (conf_pct in [0,100]). Points are + emitted grouped by source view (frames outer); if out_counts != NULL it must + point to int[n_images] and receives the per-view point count (for progressive + build-up rendering). On success sets *out_n and mallocs *out_xyz[3N] (world xyz, + OpenCV frame), *out_rgb[3N] uint8, *out_radius[N]; free xyz/radius via + da_capi_free_floats and rgb via da_capi_free_bytes. Returns 0 ok, -1 error. */ +int da_capi_points_multi(da_ctx* ctx, const char** image_paths, int n_images, + double conf_pct, float point_size, + int* out_n, int* out_counts, + float** out_xyz, unsigned char** out_rgb, float** out_radius); + +/* Sliding-window STREAMING colored point cloud (DualDPT/pose-capable DA3 models; + returns -1 for mono/DA2). For long ordered frame sequences (time-lapse video): + runs the fused cross-view pass on overlapping windows of chunk_size frames + (sharing `overlap` frames) and stitches each window into ONE global frame via a + weighted-Umeyama Sim3 solved on the overlap, so hundreds of frames fuse into a + single coherent cloud at bounded per-pass memory. conf_pct in [0,100]; + global_budget caps total emitted points (<=0 = unlimited, subsampled per window). + out_counts (if non-NULL) is int[n_images] receiving per-INPUT-frame point counts + (frame-major, for progressive build-up). On success sets *out_n and mallocs + *out_xyz[3N] (world xyz, OpenCV frame), *out_rgb[3N] uint8, *out_radius[N]; free + xyz/radius via da_capi_free_floats and rgb via da_capi_free_bytes. 0 ok, -1 err. + + De-ghosting toggles (0=off, non-zero=on): + icp_refine : point-to-plane ICP refinement of each window seam; + loop_close : loop-closure detection + Sim3 pose-graph drift removal; + fuse : final voxel/surface fusion (collapses doubled sheets, de-densifies) + with cell fuse_voxel_m metres (<=0 => 0.03 m default). After fusion + the point order is spatial, so out_counts is rescaled to sum to N. + metric : rescale each window to absolute METRES via the nested metric branch + (per-window robust-median scale_factor applied to depth + pose). + REQUIRES a da_capi_load_nested ctx; returns -1 otherwise. Without it + the cloud is at an arbitrary relative scale. + + Per-frame camera poses (for the viewer flythrough) are stashed on the ctx and + retrieved separately via da_capi_stream_last_poses — this function is already at the + FFI argument-count ceiling, so they can't be added as out-params here. */ +int da_capi_points_stream(da_ctx* ctx, const char** image_paths, int n_images, + int chunk_size, int overlap, double conf_pct, + float point_size, int global_budget, + int icp_refine, int loop_close, int fuse, int metric, + double fuse_voxel_m, + int* out_n, int* out_counts, + float** out_xyz, unsigned char** out_rgb, float** out_radius); + +/* Per-input-frame camera poses from the most recent da_capi_points_stream call. + Mallocs *out_pos[3F] (camera centre) and *out_fwd[3F] (unit view direction), OpenCV + world axes; sets *out_nframes = F (== that call's n_images). Free via + da_capi_free_floats. Returns 0 ok, -1 if none (no stream run / all windows failed). */ +int da_capi_stream_last_poses(da_ctx* ctx, float** out_pos, float** out_fwd, int* out_nframes); + +/* Single-image 3D GAUSSIANS (DA3-GIANT / GS-head models only; returns -1 with a + clear last_error otherwise). Returns world-frame (OpenCV) gaussians as parallel + arrays: *out_xyz[3N] means, *out_scale[3N], *out_quat[4N] (wxyz), *out_rgb[3N] + linear colour in [0,1] (input-photo colour; see note), *out_opacity[N]. Sets + *out_n. Also, when non-NULL, writes the input camera intrinsics to out_intr[9] + (K 3x3 row-major, pixel units) and the processed resolution to *out_w,*out_h -- + the pose is canonical (identity) so a viewer placed at the origin looking down + +z with this K reproduces the input view. Free every returned float buffer via + da_capi_free_floats. Returns 0 ok, -1 error. */ +int da_capi_gaussians(da_ctx* ctx, const char* image_path, int* out_n, + float** out_xyz, float** out_scale, float** out_quat, + float** out_rgb, float** out_opacity, + float* out_intr, int* out_w, int* out_h); #ifdef __cplusplus } #endif diff --git a/scripts/fetch_models.sh b/scripts/fetch_models.sh new file mode 100755 index 0000000..6908f6a --- /dev/null +++ b/scripts/fetch_models.sh @@ -0,0 +1,34 @@ +#!/usr/bin/env bash +# Pre-download the curated DA3 model set for the demo app (~13 GB). +# One weight per capability, not the full 31 GB of redundant quants. +# Idempotent: skips files already present at the right size; resumes partials. +set -uo pipefail + +REPO="mudler/depth-anything.cpp-gguf" +BASE="https://huggingface.co/${REPO}/resolve/main" +DIR="$(cd "$(dirname "$0")/../models" && pwd)" + +# capability file +CURATED=( + "relative+pose (fast) depth-anything-small-f32.gguf" + "relative+pose (default) depth-anything-base-q4_k.gguf" + "relative+pose (quality) depth-anything-large-f32.gguf" + "relative+pose+gaussians depth-anything-giant-f32.gguf" + "metric+pose (anyview branch) depth-anything-nested-anyview.gguf" + "metric+pose (metric branch) depth-anything-nested-metric.gguf" +) + +echo "Downloading curated DA3 models into $DIR" +for row in "${CURATED[@]}"; do + cap="${row% *}"; file="${row##* }" + out="$DIR/$file" + if [ -s "$out" ]; then + echo " [skip] $file (present, $(du -h "$out" | cut -f1))" + continue + fi + echo " [get ] $file — $cap" + curl -fL -C - --retry 5 --retry-delay 3 -o "$out" "$BASE/$file" \ + || { echo " [FAIL] $file"; rm -f "$out"; } +done +echo "Done. Models present:" +ls -lh "$DIR"/*.gguf 2>/dev/null diff --git a/scripts/oracle.sh b/scripts/oracle.sh new file mode 100755 index 0000000..dd387d7 --- /dev/null +++ b/scripts/oracle.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash +# Reference-oracle Python runner for the streaming-alignment validation suite +# (tasks A/B/C). Provides open3d (ICP + voxel fusion + pose-graph), gtsam +# (Sim3 pose-graph), and scipy/numpy — the independent implementations we +# differentially test our C++ against. +# +# NixOS note: the manylinux open3d wheel dlopen's X11/GL at import, which are +# not on the default loader path here. We assemble LD_LIBRARY_PATH from the +# store (cached next to the venv) so the wheel loads. gtsam pins numpy<2, so +# the whole stack is pinned to a numpy 1.26 / scipy 1.13 line. +# +# Usage: +# scripts/oracle.sh setup # (re)create venv + install pinned deps +# scripts/oracle.sh python script.py # run a script with the oracle env +# scripts/oracle.sh -c "import open3d" # inline +set -euo pipefail + +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +VENV="$ROOT/.oracle-venv" +PY="$VENV/bin/python" +LDP_CACHE="$VENV/.ldpath" + +# Libraries the open3d wheel needs at import (X11/GL cluster + toolchain). +NEED_LIBS=( + libX11.so.6 libXext.so.6 libXrandr.so.2 libXinerama.so.1 libXcursor.so.1 + libXi.so.6 libXfixes.so.3 libXrender.so.1 libGL.so.1 libEGL.so.1 + libGLX.so.0 libGLdispatch.so.0 libgomp.so.1 libstdc++.so.6 libgcc_s.so.1 + libxcb.so.1 libXau.so.6 libXdmcp.so.6 +) + +# True (0) iff $1 is a 64-bit ELF (byte 4 of the header == 2). The store also +# holds 32-bit multilib copies (lib32/) that must not leak onto the path. +is_elf64() { + [ "$(od -An -t u1 -j4 -N1 "$1" 2>/dev/null | tr -d ' ')" = "2" ] +} + +compute_ldpath() { + local dirs="" cand + for lib in "${NEED_LIBS[@]}"; do + while IFS= read -r cand; do + if is_elf64 "$cand"; then dirs="$dirs:$(dirname "$cand")"; break; fi + done < <(find /nix/store -maxdepth 3 -name "$lib" 2>/dev/null) + done + echo "$dirs" | tr ':' '\n' | sort -u | grep -v '^$' | paste -sd: +} + +ensure_ldpath() { + if [ ! -s "$LDP_CACHE" ]; then compute_ldpath > "$LDP_CACHE"; fi + cat "$LDP_CACHE" +} + +case "${1:-python}" in + setup) + uv venv --python 3.12 "$VENV" + uv pip install --python "$PY" \ + "numpy==1.26.4" "scipy==1.13.1" "open3d==0.19.0" "gtsam==4.2.1" + compute_ldpath > "$LDP_CACHE" + echo "oracle env ready: $VENV" + ;; + python) + shift + LD_LIBRARY_PATH="$(ensure_ldpath):${LD_LIBRARY_PATH:-}" exec "$PY" "$@" + ;; + *) + # treat all args as python args (e.g. -c "...") + LD_LIBRARY_PATH="$(ensure_ldpath):${LD_LIBRARY_PATH:-}" exec "$PY" "$@" + ;; +esac diff --git a/scripts/oracle_fuse.py b/scripts/oracle_fuse.py new file mode 100644 index 0000000..f25d549 --- /dev/null +++ b/scripts/oracle_fuse.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# Differential oracle for task A (src/fuse.cpp). Diffs our C++ fusion against +# reference implementations on a SURFACE-concentrated cloud (planes + sphere): +# * voxel stage vs open3d.voxel_down_sample +# * radius stage vs an independent scipy cKDTree neighbourhood-centroid +# Run via: scripts/oracle.sh python scripts/oracle_fuse.py +import os, struct, subprocess, sys, tempfile +import numpy as np +import open3d as o3d +from scipy.spatial import cKDTree + +DUMP = sys.argv[1] if len(sys.argv) > 1 else "./build/tests/fuse_dump" + +def surface_cloud(seed=0): + rng = np.random.default_rng(seed) + parts = [] + # floor z=0, +x wall x=2, -y wall y=-2 (each a thin noisy sheet) + xy = rng.uniform(-2, 2, (20000, 2)); parts.append(np.c_[xy, rng.normal(0, 0.004, 20000)]) + yz = rng.uniform(-2, 2, (20000, 2)); parts.append(np.c_[2 + rng.normal(0, 0.004, 20000), yz]) + xz = rng.uniform(-2, 2, (16000, 2)); parts.append(np.c_[xz[:,0], -2 + rng.normal(0, 0.004, 16000), xz[:,1]]) + d = rng.normal(size=(14000, 3)); d /= np.linalg.norm(d, axis=1, keepdims=True) # sphere r=1 + parts.append(d) + return np.vstack(parts).astype(np.float32) + +def run_dump(P, voxel, radius=0.0): + with tempfile.TemporaryDirectory() as td: + ip, op = os.path.join(td, "in.bin"), os.path.join(td, "out.bin") + with open(ip, "wb") as f: + f.write(struct.pack(" order preserved, N out +tree = cKDTree(Ps) +ref_s = np.empty_like(Ps, dtype=np.float64) +nbrs = tree.query_ball_point(Ps, r) +for i, nb in enumerate(nbrs): + ref_s[i] = Ps[nb].mean(axis=0) +maxerr = np.max(np.linalg.norm(ours_s - ref_s, axis=1)) +print(f"[radius] N={len(Ps)} max |ours - scipy_centroid| = {maxerr*1e6:.2f} um") +check(len(ours_s) == len(Ps), "radius stage preserves point count/order") +check(maxerr < 1e-4, "radius smoothing matches scipy neighbourhood centroid") + +print(("ORACLE PASS" if fails == 0 else f"ORACLE FAIL ({fails})")) +sys.exit(1 if fails else 0) diff --git a/scripts/oracle_icp.py b/scripts/oracle_icp.py new file mode 100644 index 0000000..3f6efef --- /dev/null +++ b/scripts/oracle_icp.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +# Differential oracle for task B (src/icp.cpp). On SURFACE-concentrated clouds it +# diffs our point-to-plane ICP against open3d.registration_icp (also point-to- +# plane) AND against the known ground-truth transform: +# * curvature-rich corner+sphere: both recover a KNOWN rigid transform, and our +# result agrees with open3d's to <0.1 deg / sub-mm; +# * single flat wall: both drive the point-to-plane residual to ~0 (the +# observable normal error), confirming the fit is correct where it is +# constrained (our rank-truncation of the unobservable tangential slide is +# asserted separately in tests/test_icp.cpp). +# Run via: scripts/oracle.sh python scripts/oracle_icp.py +import os, struct, subprocess, sys, tempfile +import numpy as np +import open3d as o3d +from scipy.spatial.transform import Rotation + +DUMP = sys.argv[1] if len(sys.argv) > 1 else "./build/tests/icp_dump" +MAXC, NRAD = 0.10, 0.06 + +fails = 0 +def check(ok, msg): + global fails + print(("ok: " if ok else "FAIL: ") + msg) + if not ok: fails += 1 + +def corner_scene(seed=0): + rng = np.random.default_rng(seed) + xy = rng.uniform(-0.5, 0.5, (3000, 2)); floor = np.c_[xy[:,0]+0.5, xy[:,1], np.zeros(3000)] + yz = rng.uniform(-0.5, 0.5, (3000, 2)); wall = np.c_[np.zeros(3000), yz[:,0], yz[:,1]+0.5] + d = rng.normal(size=(2500,3)); d /= np.linalg.norm(d, axis=1, keepdims=True) + sph = np.c_[d[:,0]*0.22+0.5, d[:,1]*0.22, d[:,2]*0.22+0.7] + return np.vstack([floor, wall, sph]).astype(np.float32) + +def wall_scene(seed=1): + rng = np.random.default_rng(seed) + xy = rng.uniform(-0.6, 0.6, (8000, 2)) + return np.c_[xy[:,0], xy[:,1], np.zeros(8000)].astype(np.float32) + +def write_cloud(path, P): + with open(path, "wb") as f: + f.write(struct.pack(" T ~ G^-1 +Ginv = np.linalg.inv(G) + +with tempfile.TemporaryDirectory() as td: + T_ours, s, rms, meta = run_ours(src, tgt, td) +T_o3d, reg = run_o3d(src, tgt) + +ang_ours_gt = rot_angle_deg(T_ours[:3,:3], Ginv[:3,:3]); t_ours_gt = np.linalg.norm(T_ours[:3,3]-Ginv[:3,3]) +ang_o3d_gt = rot_angle_deg(T_o3d[:3,:3], Ginv[:3,:3]); t_o3d_gt = np.linalg.norm(T_o3d[:3,3]-Ginv[:3,3]) +ang_pair = rot_angle_deg(T_ours[:3,:3], T_o3d[:3,:3]); t_pair = np.linalg.norm(T_ours[:3,3]-T_o3d[:3,3]) +print(f"[corner] ours vs GT : {ang_ours_gt:.4f} deg {t_ours_gt*1e3:.3f} mm (rank={meta[0]} iters={meta[1]} s={s:.4f})") +print(f"[corner] o3d vs GT : {ang_o3d_gt:.4f} deg {t_o3d_gt*1e3:.3f} mm (fitness={reg.fitness:.3f} rmse={reg.inlier_rmse*1e3:.3f}mm)") +print(f"[corner] ours vs o3d: {ang_pair:.4f} deg {t_pair*1e3:.3f} mm") +check(ang_ours_gt < 0.1 and t_ours_gt < 1e-3, "ours recovers known transform (<0.1deg, <1mm)") +check(ang_o3d_gt < 0.1 and t_o3d_gt < 1e-3, "open3d recovers known transform (<0.1deg, <1mm)") +check(ang_pair < 0.1 and t_pair < 1e-3, "ours agrees with open3d (<0.1deg, <1mm)") +check(rms[1] < 1e-3, "ours point-to-plane residual converged to ~0") + +# ================= 2. flat wall: both drive plane residual to ~0 ============ +wt = wall_scene() +Rw = Rotation.from_rotvec(np.radians(1.0) * np.array([1.0,0,0])) +Gw = np.eye(4); Gw[:3,:3] = Rw.as_matrix(); Gw[:3,3] = [0.05, 0.0, 0.03] # tangential x + normal z + tilt +ws = apply4(Gw, wt.astype(np.float64)).astype(np.float32) + +with tempfile.TemporaryDirectory() as td: + Tw_ours, _, rms_w, meta_w = run_ours(ws, wt, td) +Tw_o3d, reg_w = run_o3d(ws, wt) + +# residual to the true plane z=0 after each transform (the OBSERVABLE error). +def plane_resid_z(T, P): + return float(np.sqrt(np.mean(apply4(T, P.astype(np.float64))[:,2]**2))) +rz_ours, rz_o3d = plane_resid_z(Tw_ours, ws), plane_resid_z(Tw_o3d, ws) +print(f"[wall] plane resid ours={rz_ours*1e3:.3f}mm o3d={rz_o3d*1e3:.3f}mm (ours rank={meta_w[0]}, o3d rmse={reg_w.inlier_rmse*1e3:.3f}mm)") +check(rz_ours < 2e-3, "ours corrects the observable normal error (plane residual ~0)") +check(rz_o3d < 2e-3, "open3d corrects the observable normal error (plane residual ~0)") +check(meta_w[0] == 3, "ours flags the wall as rank-3 (aperture problem)") + +print("ORACLE PASS" if fails == 0 else f"ORACLE FAIL ({fails})") +sys.exit(1 if fails else 0) diff --git a/scripts/oracle_posegraph.py b/scripts/oracle_posegraph.py new file mode 100644 index 0000000..550bb76 --- /dev/null +++ b/scripts/oracle_posegraph.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 +# Differential oracle for task C (src/posegraph.cpp). Cross-checks our Sim3 pose- +# graph optimizer against gtsam's Pose3 pose-graph (BetweenFactorPose3 + +# PriorFactorPose3 + LevenbergMarquardt) on the SE3 (scale=1) case, which is what +# gtsam 4.2's Python wrapper can express (it has no BetweenFactorSimilarity3). +# 1. consistent ring + loop + perturbed init -> both recover the KNOWN poses, +# and our result agrees with gtsam node-for-node; +# 2. systematic drift + strong loop closure -> both cut ATE the same way. +# The Sim3 SCALE dimension (which gtsam Python can't graph-optimize) is validated +# analytically in tests/test_posegraph.cpp. +# scripts/oracle.sh python scripts/oracle_posegraph.py +import os, struct, subprocess, sys, tempfile +import numpy as np +import gtsam +from scipy.spatial.transform import Rotation + +DUMP = sys.argv[1] if len(sys.argv) > 1 else "./build/tests/posegraph_dump" +np.random.seed(7) + +fails = 0 +def check(ok, msg): + global fails + print(("ok: " if ok else "FAIL: ") + msg) + if not ok: fails += 1 + +# ---- SE3 helpers (4x4 homogeneous) --------------------------------------- +def se3(R, t): + M = np.eye(4); M[:3,:3] = R; M[:3,3] = t; return M +def inv(M): + R = M[:3,:3]; t = M[:3,3]; o = np.eye(4); o[:3,:3] = R.T; o[:3,3] = -R.T @ t; return o +def rand_se3(tsc, rsc): + return se3(Rotation.from_rotvec(rsc*np.random.randn(3)).as_matrix(), tsc*np.random.randn(3)) +def pose_diff(A, B): + dt = np.linalg.norm(A[:3,3] - B[:3,3]) + Rr = A[:3,:3].T @ B[:3,:3] + ang = np.degrees(np.arccos(np.clip((np.trace(Rr)-1)/2, -1, 1))) + return dt, ang +def gt_loop(N, radius): + P = [] + for i in range(N): + a = 2*np.pi*i/N + R = Rotation.from_rotvec(a*np.array([0,0,1.0])).as_matrix() + P.append(se3(R, [radius*np.cos(a), radius*np.sin(a), 0.2*np.sin(2*a)])) + return P + +# ---- pose-graph (de)serialization for our dump --------------------------- +def pack_T(M): # SE3 as Sim3 with s=1: 13 f64 [s, R0..8, t0..2] + R = M[:3,:3].reshape(-1); t = M[:3,3] + return struct.pack("<13d", 1.0, *R, *t) +def unpack_T(buf): + b = struct.unpack("<13d", buf); R = np.array(b[1:10]).reshape(3,3) + M = np.eye(4); M[:3,:3] = R; M[:3,3] = b[10:13]; return M +def run_ours(nodes, edges, fixed, td): + ip, op = os.path.join(td,"in.bin"), os.path.join(td,"out.bin") + with open(ip,"wb") as f: + f.write(struct.pack("{c:.2e}, it={it})") +print(f"[exact] gtsam vs GT: {d_gt[0]*1e3:.4f} mm {d_gt[1]:.5f} deg") +print(f"[exact] ours vs gts: {d_pair[0]*1e3:.4f} mm {d_pair[1]:.5f} deg") +check(d_ours[0] < 1e-4 and d_ours[1] < 1e-3, "ours recovers known poses (<0.1mm, <1e-3deg)") +check(d_gt[0] < 1e-4 and d_gt[1] < 1e-3, "gtsam recovers known poses (<0.1mm, <1e-3deg)") +check(d_pair[0] < 1e-4 and d_pair[1] < 1e-3, "ours agrees with gtsam node-for-node") + +# ================= 2. systematic drift + strong loop closure ================ +N = 16; GT = gt_loop(N, 3.0) +bias = se3(Rotation.from_rotvec(0.010*np.array([0,0,1.0])).as_matrix(), [0.010, 0, 0]) +edges = []; nodes = [GT[0].copy()] +for i in range(N-1): + Z = rel(GT[i], GT[i+1]) @ bias @ rand_se3(0.004, 0.004) # systematic + small iid + edges.append((i, i+1, 1.0, Z)) + nodes.append(nodes[i] @ Z) # chain => accumulates drift +edges.append((N-1, 0, 25.0, rel(GT[N-1], GT[0]))) # accurate loop closure +fixed = [0] +with tempfile.TemporaryDirectory() as td: + ours2, _, _, _ = run_ours(nodes, edges, fixed, td) +gt2 = run_gtsam(nodes, edges, fixed) +def ate(P): return np.sqrt(np.mean([np.linalg.norm(P[i][:3,3]-GT[i][:3,3])**2 for i in range(N)])) +a_open, a_ours, a_gt = ate(nodes), ate(ours2), ate(gt2) +d_pair2 = max(pose_diff(ours2[i], gt2[i]) for i in range(N)) +# On INCONSISTENT data the two optima differ by the parametrization of the +# translation error (we use E.t directly; gtsam's Pose3 Logmap folds in the SE3 +# left-Jacobian), so we cross-check EQUIVALENT BEHAVIOUR — both cut the drift to +# the same degree and agree to a small fraction of the corrected drift — not a +# bit-identical trajectory. +print(f"[drift] ATE open={a_open*1e3:.2f}mm ours={a_ours*1e3:.2f}mm gtsam={a_gt*1e3:.2f}mm") +print(f"[drift] ours vs gtsam: {d_pair2[0]*1e3:.3f} mm ({100*d_pair2[0]/a_open:.1f}% of drift) {d_pair2[1]:.4f} deg") +check(a_ours < 0.3*a_open, "ours: loop closure cuts systematic-drift ATE by >70%") +check(a_gt < 0.3*a_open, "gtsam: loop closure cuts systematic-drift ATE by >70%") +check(abs(a_ours-a_gt) < 0.2*max(a_ours,a_gt), "ours & gtsam fix the drift to the same degree (ATE within 20%)") +check(d_pair2[0] < 0.05*a_open and d_pair2[1] < 0.1, "ours agrees with gtsam to <5% of the corrected drift, <0.1deg") + +print("ORACLE PASS" if fails == 0 else f"ORACLE FAIL ({fails})") +sys.exit(1 if fails else 0) diff --git a/scripts/stream_regress.sh b/scripts/stream_regress.sh new file mode 100755 index 0000000..27d699d --- /dev/null +++ b/scripts/stream_regress.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash +# Real-clip regression for the streaming de-ghosting toggles (tasks A/B/C). +# Extracts frames from a video (ffmpeg, mirroring the demo server's fps/scale) and +# runs build/tests/stream_regress over the A/B/C variants, printing per-variant +# point count, GT-free surface thickness (ghosting proxy), and wall-clock time. +# +# Needs a pose-capable DA3 model (e.g. models/depth-anything-giant-f32.gguf). +# Build the harness first: +# cmake --build build --target stream_regress +# Usage: +# scripts/stream_regress.sh