crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: ignore boulder.json and update extrinsics visualizer

Browse Source
This commit is contained in:
crosstyan
2026-02-07 15:35:48 +00:00
parent a8d375191a
commit 0d3e9e67ad
7 changed files with 102 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/aruco/depth_refine.py
+30 -20
View File
@@ -1,5 +1,5 @@
import numpy as np
from typing import Dict, Tuple, Any, Optional
from typing import Dict, Tuple, Any, Optional, List
from scipy.optimize import least_squares
from .pose_math import rvec_tvec_to_matrix, matrix_to_rvec_tvec
from .depth_verify import (
@@ -22,7 +22,7 @@ def params_to_extrinsics(params: np.ndarray) -> np.ndarray:
def depth_residuals(
params: np.ndarray,
marker_corners_world: Dict[int, np.ndarray],
active_corners: List[np.ndarray],
depth_map: np.ndarray,
K: np.ndarray,
initial_params: np.ndarray,
@@ -34,21 +34,25 @@ def depth_residuals(
T = params_to_extrinsics(params)
residuals = []
for marker_id, corners in marker_corners_world.items():
for corner in corners:
residual = compute_depth_residual(corner, T, depth_map, K, window_size=5)
if residual is not None:
if confidence_map is not None:
u, v = project_point_to_pixel(
(np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K
)
if u is not None and v is not None:
h, w = confidence_map.shape[:2]
if 0 <= u < w and 0 <= v < h:
conf = confidence_map[v, u]
weight = get_confidence_weight(conf, confidence_thresh)
residual *= np.sqrt(weight)
residuals.append(residual)
for corner in active_corners:
residual = compute_depth_residual(corner, T, depth_map, K, window_size=5)
if residual is None:
# If a point becomes invalid during optimization, assign a large residual
# to discourage the optimizer from going there, but keep the vector length fixed.
# 10.0m is a safe "large" value for depth residuals.
residual = 10.0
else:
if confidence_map is not None:
u, v = project_point_to_pixel(
(np.linalg.inv(T) @ np.append(corner, 1.0))[:3], K
)
if u is not None and v is not None:
h, w = confidence_map.shape[:2]
if 0 <= u < w and 0 <= v < h:
conf = confidence_map[v, u]
weight = get_confidence_weight(conf, confidence_thresh)
residual *= np.sqrt(weight)
residuals.append(residual)
# Regularization as pseudo-residuals
param_diff = params - initial_params
@@ -90,6 +94,7 @@ def refine_extrinsics_with_depth(
reg_trans = regularization_weight * 10.0
# Check for valid depth points first
active_corners = []
n_points_total = 0
n_depth_valid = 0
n_confidence_rejected = 0
@@ -100,6 +105,7 @@ def refine_extrinsics_with_depth(
res = compute_depth_residual(corner, T_initial, depth_map, K, window_size=5)
if res is not None:
n_depth_valid += 1
is_confident = True
if confidence_map is not None:
u, v = project_point_to_pixel(
(np.linalg.inv(T_initial) @ np.append(corner, 1.0))[:3], K
@@ -111,8 +117,12 @@ def refine_extrinsics_with_depth(
weight = get_confidence_weight(conf, confidence_thresh)
if weight <= 0:
n_confidence_rejected += 1
is_confident = False
if n_depth_valid == 0:
if is_confident:
active_corners.append(corner)
if not active_corners:
return T_initial, {
"success": False,
"reason": "no_valid_depth_points",
@@ -152,7 +162,7 @@ def refine_extrinsics_with_depth(
depth_residuals,
initial_params,
args=(
marker_corners_world,
active_corners,
depth_map,
K,
initial_params,
@@ -179,7 +189,7 @@ def refine_extrinsics_with_depth(
# Calculate initial cost for comparison
initial_residuals = depth_residuals(
initial_params,
marker_corners_world,
active_corners,
depth_map,
K,
initial_params,