1467 lines
52 KiB
Python
1467 lines
52 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
3_multiview_bundle_adjustment_v3.py
|
|
|
|
Multi-view ArUco marker position optimization with rule-based geometric constraints.
|
|
|
|
Mathematical model
|
|
------------------
|
|
We estimate 3D marker positions X_i ∈ R^3 by minimizing a weighted least-squares objective
|
|
|
|
E(X) = Σ_i Σ_c w_ic || π_c(X_i) - u_ic ||^2
|
|
+ λ_d Σ_j w_j^d || g_j^d(X) ||^2
|
|
+ λ_a Σ_k w_k^a || g_k^a(X) ||^2
|
|
|
|
where:
|
|
- u_ic are observed normalized image coordinates of marker i in camera c
|
|
- π_c(.) is the normalized reprojection model for camera c
|
|
- w_ic are observation weights (quality-based)
|
|
- g_j^d are rigid-link distance constraints
|
|
- g_k^a are joint-axis / chain-axis projection constraints
|
|
|
|
Important design choices:
|
|
- robot.json is used as a kinematic description, not as a direct source of marker positions.
|
|
- constraint families are explicit and switchable.
|
|
- marker observation weights are based on available measurement quality cues
|
|
(e.g. detection confidence, observed pixel size if present, marker size prior,
|
|
and initial range from the camera); orientation information is treated cautiously.
|
|
- optimization is single-stage in this version to keep comparisons reproducible.
|
|
If a second refinement stage is needed later, it can be added as a separate version.
|
|
|
|
Dependencies:
|
|
numpy, opencv-python, scipy (optional for optimization)
|
|
|
|
Example:
|
|
python 3_multiview_bundle_adjustment_v3.py ^
|
|
-det cam1_aruco_detection.json cam2_aruco_detection.json cam3_aruco_detection.json ^
|
|
-pose cam1_camera_pose.json cam2_camera_pose.json cam3_camera_pose.json ^
|
|
-robot robot.json ^
|
|
-lambdaWeight 100.0
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import os
|
|
import sys
|
|
import time
|
|
from dataclasses import dataclass
|
|
from itertools import combinations
|
|
from typing import Any, Dict, List, Optional, Tuple
|
|
|
|
import cv2
|
|
import numpy as np
|
|
|
|
|
|
# ===================================================================
|
|
# Path / JSON helpers
|
|
# ===================================================================
|
|
|
|
def resolve_path(path: str) -> str:
|
|
path = os.path.expanduser(path)
|
|
if os.path.isabs(path):
|
|
return path
|
|
return os.path.abspath(path)
|
|
|
|
|
|
def load_json(path: str) -> Dict[str, Any]:
|
|
with open(resolve_path(path), "r", encoding="utf-8") as f:
|
|
return json.load(f)
|
|
|
|
|
|
def save_json(path: str, data: Dict[str, Any]) -> None:
|
|
with open(resolve_path(path), "w", encoding="utf-8") as f:
|
|
json.dump(data, f, indent=2)
|
|
|
|
|
|
# ===================================================================
|
|
# Units
|
|
# ===================================================================
|
|
|
|
def get_length_scale(robot_data: Dict[str, Any]) -> float:
|
|
units = robot_data.get("units", {}) or {}
|
|
length_unit = str(units.get("length", "")).strip().lower()
|
|
if length_unit in ("mm", "millimeter", "millimeters"):
|
|
return 1.0 / 1000.0
|
|
if length_unit in ("cm", "centimeter", "centimeters"):
|
|
return 1.0 / 100.0
|
|
return 1.0
|
|
|
|
|
|
# ===================================================================
|
|
# Small geometry helpers
|
|
# ===================================================================
|
|
|
|
def safe_norm(v: np.ndarray, eps: float = 1e-12) -> float:
|
|
return float(np.linalg.norm(v) + eps)
|
|
|
|
|
|
def normalize_vector(v: np.ndarray, eps: float = 1e-12) -> np.ndarray:
|
|
return np.asarray(v, dtype=np.float64) / safe_norm(v, eps)
|
|
|
|
|
|
def clamp(v: float, lo: float, hi: float) -> float:
|
|
return float(max(lo, min(hi, v)))
|
|
|
|
|
|
def principal_axis_id(axis: np.ndarray, threshold: float = 0.95) -> Optional[int]:
|
|
"""Return 0,1,2 for x,y,z if axis is close enough to a principal axis."""
|
|
a = normalize_vector(np.asarray(axis, dtype=np.float64))
|
|
idx = int(np.argmax(np.abs(a)))
|
|
if abs(a[idx]) >= threshold:
|
|
return idx
|
|
return None
|
|
|
|
|
|
def camera_center_from_world_to_cam(R_wc: np.ndarray, t_wc: np.ndarray) -> np.ndarray:
|
|
"""world_to_camera: X_cam = R_wc * X_world + t_wc; camera center is -R^T t."""
|
|
return -R_wc.T @ t_wc
|
|
|
|
|
|
def principal_axis_vector(axis: np.ndarray) -> np.ndarray:
|
|
"""Convert a near-principal axis to an exact signed principal axis vector."""
|
|
a = normalize_vector(axis)
|
|
idx = int(np.argmax(np.abs(a)))
|
|
out = np.zeros(3, dtype=np.float64)
|
|
out[idx] = 1.0 if a[idx] >= 0 else -1.0
|
|
return normalize_vector(out)
|
|
|
|
|
|
# ===================================================================
|
|
# Configuration
|
|
# ===================================================================
|
|
|
|
@dataclass
|
|
class ConstraintRuleConfig:
|
|
rigid_distance_enabled: bool = True
|
|
rigid_distance_mode: str = "mst" # mst | star | full
|
|
rigid_distance_weight: float = 1.0
|
|
|
|
joint_axis_enabled: bool = True
|
|
joint_axis_max_pairs: int = 2
|
|
joint_axis_weight: float = 0.5
|
|
|
|
chain_axis_enabled: bool = True
|
|
chain_axis_max_depth: int = 3
|
|
chain_axis_max_pairs: int = 2
|
|
chain_axis_weight: float = 0.3
|
|
|
|
axis_alignment_threshold: float = 0.95
|
|
|
|
strict_unique_marker_ids: bool = False
|
|
show_skipped_constraints: bool = True
|
|
|
|
enable_observation_weights: bool = True
|
|
weight_floor: float = 0.30
|
|
weight_ceiling: float = 3.00
|
|
ref_distance_m: float = 0.75
|
|
ref_marker_size_px: float = 50.0
|
|
use_detection_confidence: bool = True
|
|
use_detection_size_px: bool = True
|
|
use_initial_range: bool = True
|
|
use_marker_size_prior: bool = True
|
|
|
|
|
|
def _bool_or_default(value: Any, default: bool) -> bool:
|
|
if value is None:
|
|
return default
|
|
return bool(value)
|
|
|
|
|
|
def _float_or_default(value: Any, default: float) -> float:
|
|
if value is None:
|
|
return default
|
|
return float(value)
|
|
|
|
|
|
def _int_or_default(value: Any, default: int) -> int:
|
|
if value is None:
|
|
return default
|
|
return int(value)
|
|
|
|
|
|
def load_constraint_rule_config(robot_data: Dict[str, Any], args: argparse.Namespace) -> ConstraintRuleConfig:
|
|
rules = robot_data.get("constraint_rules", {}) or {}
|
|
|
|
cfg = ConstraintRuleConfig()
|
|
rigid = rules.get("rigid_distance", {}) or {}
|
|
joint = rules.get("joint_axis_projection", {}) or {}
|
|
chain = rules.get("chain_axis_projection", {}) or {}
|
|
obs = rules.get("observation_weights", {}) or {}
|
|
|
|
cfg.rigid_distance_enabled = _bool_or_default(rigid.get("enabled"), cfg.rigid_distance_enabled)
|
|
cfg.rigid_distance_mode = str(rigid.get("mode", cfg.rigid_distance_mode)).strip().lower()
|
|
cfg.rigid_distance_weight = _float_or_default(rigid.get("weight"), cfg.rigid_distance_weight)
|
|
|
|
cfg.joint_axis_enabled = _bool_or_default(joint.get("enabled"), cfg.joint_axis_enabled)
|
|
cfg.joint_axis_max_pairs = _int_or_default(joint.get("max_pairs"), cfg.joint_axis_max_pairs)
|
|
cfg.joint_axis_weight = _float_or_default(joint.get("weight"), cfg.joint_axis_weight)
|
|
|
|
cfg.chain_axis_enabled = _bool_or_default(chain.get("enabled"), cfg.chain_axis_enabled)
|
|
cfg.chain_axis_max_depth = _int_or_default(chain.get("max_depth"), cfg.chain_axis_max_depth)
|
|
cfg.chain_axis_max_pairs = _int_or_default(chain.get("max_pairs"), cfg.chain_axis_max_pairs)
|
|
cfg.chain_axis_weight = _float_or_default(chain.get("weight"), cfg.chain_axis_weight)
|
|
|
|
cfg.axis_alignment_threshold = _float_or_default(
|
|
rules.get("axis_alignment_threshold"), cfg.axis_alignment_threshold
|
|
)
|
|
|
|
cfg.enable_observation_weights = _bool_or_default(obs.get("enabled"), cfg.enable_observation_weights)
|
|
cfg.weight_floor = _float_or_default(obs.get("weight_floor"), cfg.weight_floor)
|
|
cfg.weight_ceiling = _float_or_default(obs.get("weight_ceiling"), cfg.weight_ceiling)
|
|
cfg.ref_distance_m = _float_or_default(obs.get("ref_distance_m"), cfg.ref_distance_m)
|
|
cfg.ref_marker_size_px = _float_or_default(obs.get("ref_marker_size_px"), cfg.ref_marker_size_px)
|
|
cfg.use_detection_confidence = _bool_or_default(obs.get("use_detection_confidence"), cfg.use_detection_confidence)
|
|
cfg.use_detection_size_px = _bool_or_default(obs.get("use_detection_size_px"), cfg.use_detection_size_px)
|
|
cfg.use_initial_range = _bool_or_default(obs.get("use_initial_range"), cfg.use_initial_range)
|
|
cfg.use_marker_size_prior = _bool_or_default(obs.get("use_marker_size_prior"), cfg.use_marker_size_prior)
|
|
|
|
if getattr(args, "strictUniqueMarkerIds", False):
|
|
cfg.strict_unique_marker_ids = True
|
|
if getattr(args, "showSkippedConstraints", False):
|
|
cfg.show_skipped_constraints = True
|
|
if getattr(args, "noShowSkippedConstraints", False):
|
|
cfg.show_skipped_constraints = False
|
|
|
|
return cfg
|
|
|
|
|
|
# ===================================================================
|
|
# Observation / constraint definitions
|
|
# ===================================================================
|
|
|
|
@dataclass
|
|
class Observation:
|
|
cam_idx: int
|
|
norm_coords: np.ndarray
|
|
meta: Dict[str, Any]
|
|
|
|
|
|
@dataclass
|
|
class MarkerDistanceConstraint:
|
|
marker_id_a: int
|
|
marker_id_b: int
|
|
link_name: str
|
|
target_distance_m: float
|
|
weight: float = 1.0
|
|
enabled: bool = True
|
|
source: str = "rigid_distance"
|
|
|
|
|
|
@dataclass
|
|
class JointAxisConstraint:
|
|
marker_id_parent: int
|
|
marker_id_child: int
|
|
parent_link: str
|
|
child_link: str
|
|
joint_axis: np.ndarray
|
|
target_delta_along_axis_m: float
|
|
weight: float = 1.0
|
|
enabled: bool = True
|
|
source: str = "joint_axis_projection"
|
|
|
|
|
|
Constraint = MarkerDistanceConstraint | JointAxisConstraint
|
|
|
|
|
|
# ===================================================================
|
|
# Robot parsing
|
|
# ===================================================================
|
|
|
|
def parse_robot_markers(
|
|
robot_data: Dict[str, Any],
|
|
length_scale: float,
|
|
strict_unique_marker_ids: bool = False
|
|
) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[str], Dict[int, Dict[str, Any]]]:
|
|
links = robot_data.get("links", {}) or {}
|
|
|
|
marker_to_link: Dict[int, str] = {}
|
|
link_markers: Dict[str, List[Dict[str, Any]]] = {}
|
|
issues: List[str] = []
|
|
marker_meta: Dict[int, Dict[str, Any]] = {}
|
|
|
|
seen_global: Dict[int, str] = {}
|
|
|
|
for link_name, link_data in links.items():
|
|
markers = link_data.get("markers", []) or []
|
|
collected: List[Dict[str, Any]] = []
|
|
seen_local: set[int] = set()
|
|
|
|
for idx, marker in enumerate(markers):
|
|
marker_id = int(marker.get("id", -1))
|
|
pos = marker.get("position", None)
|
|
|
|
if marker_id < 0 or pos is None or len(pos) != 3:
|
|
issues.append(f"[WARN] link={link_name}: skipped invalid marker entry at index {idx}")
|
|
continue
|
|
|
|
if marker_id in seen_local:
|
|
msg = f"[WARN] duplicate marker id {marker_id} inside link '{link_name}'"
|
|
if strict_unique_marker_ids:
|
|
raise ValueError(msg)
|
|
issues.append(msg + " -> skipped duplicate inside same link")
|
|
continue
|
|
|
|
if marker_id in seen_global:
|
|
msg = (
|
|
f"[WARN] duplicate marker id {marker_id} appears in link '{link_name}' "
|
|
f"and already in link '{seen_global[marker_id]}'"
|
|
)
|
|
if strict_unique_marker_ids:
|
|
raise ValueError(msg)
|
|
issues.append(msg + " -> skipped duplicate occurrence")
|
|
continue
|
|
|
|
seen_local.add(marker_id)
|
|
seen_global[marker_id] = link_name
|
|
|
|
pos_raw = np.array(pos, dtype=np.float64)
|
|
pos_m = pos_raw * float(length_scale)
|
|
|
|
item = {
|
|
"id": marker_id,
|
|
"name": marker.get("name", f"marker_{marker_id}"),
|
|
"position_raw": pos_raw,
|
|
"position_m": pos_m,
|
|
"normal": np.array(marker.get("normal", [0, 0, 1]), dtype=np.float64),
|
|
"size": marker.get("size", None),
|
|
"spin": marker.get("spin", None),
|
|
}
|
|
collected.append(item)
|
|
marker_to_link[marker_id] = link_name
|
|
marker_meta[marker_id] = {
|
|
"link_name": link_name,
|
|
"name": item["name"],
|
|
"position_m": pos_m,
|
|
"normal": item["normal"],
|
|
"size": item["size"],
|
|
"spin": item["spin"],
|
|
}
|
|
|
|
link_markers[link_name] = collected
|
|
|
|
return marker_to_link, link_markers, issues, marker_meta
|
|
|
|
|
|
def get_link_parent_map(robot_data: Dict[str, Any]) -> Dict[str, Optional[str]]:
|
|
links = robot_data.get("links", {}) or {}
|
|
return {link_name: (link_data.get("parent", None)) for link_name, link_data in links.items()}
|
|
|
|
|
|
def get_joint_info(robot_data: Dict[str, Any], child_link: str) -> Dict[str, Any]:
|
|
links = robot_data.get("links", {}) or {}
|
|
return (links.get(child_link, {}) or {}).get("jointToParent", {}) or {}
|
|
|
|
|
|
def get_joint_axis(robot_data: Dict[str, Any], child_link: str) -> Optional[np.ndarray]:
|
|
joint = get_joint_info(robot_data, child_link)
|
|
axis = joint.get("axis", None)
|
|
if axis is None:
|
|
return None
|
|
axis = np.asarray(axis, dtype=np.float64)
|
|
if safe_norm(axis) < 1e-12:
|
|
return None
|
|
return normalize_vector(axis)
|
|
|
|
|
|
def get_vision_marker_size_default(robot_data: Dict[str, Any]) -> float:
|
|
vision = robot_data.get("vision_config", {}) or {}
|
|
ms = vision.get("MarkerSize", None)
|
|
if ms is None:
|
|
return 0.025
|
|
return float(ms)
|
|
|
|
|
|
# ===================================================================
|
|
# Constraint compilation helpers
|
|
# ===================================================================
|
|
|
|
def get_enabled_link_rule(
|
|
robot_data: Dict[str, Any],
|
|
link_name: str,
|
|
rule_name: str,
|
|
default_enabled: bool = True
|
|
) -> bool:
|
|
overrides = robot_data.get("constraint_overrides", {}) or {}
|
|
link_override = overrides.get(link_name, {}) or {}
|
|
rule_override = link_override.get(rule_name, {}) or {}
|
|
if "enabled" in rule_override:
|
|
return bool(rule_override["enabled"])
|
|
return default_enabled
|
|
|
|
|
|
def select_anchor_marker_ids(
|
|
markers: List[Dict[str, Any]],
|
|
axis: Optional[np.ndarray] = None,
|
|
max_count: int = 2
|
|
) -> List[int]:
|
|
if not markers:
|
|
return []
|
|
if len(markers) == 1:
|
|
return [int(markers[0]["id"])]
|
|
|
|
ids = [int(m["id"]) for m in markers]
|
|
pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0)
|
|
|
|
selected: List[int] = []
|
|
|
|
if axis is not None and safe_norm(axis) > 1e-12:
|
|
a = normalize_vector(axis)
|
|
proj = pos @ a
|
|
min_idx = int(np.argmin(proj))
|
|
max_idx = int(np.argmax(proj))
|
|
selected = [ids[min_idx], ids[max_idx]]
|
|
else:
|
|
centroid = np.mean(pos, axis=0)
|
|
d = np.linalg.norm(pos - centroid, axis=1)
|
|
min_idx = int(np.argmin(d))
|
|
max_idx = int(np.argmax(d))
|
|
selected = [ids[min_idx], ids[max_idx]]
|
|
|
|
if len(selected) < max_count:
|
|
for mid in ids:
|
|
if mid not in selected:
|
|
selected.append(mid)
|
|
if len(selected) >= max_count:
|
|
break
|
|
|
|
out: List[int] = []
|
|
seen: set[int] = set()
|
|
for mid in selected:
|
|
if mid not in seen:
|
|
seen.add(mid)
|
|
out.append(mid)
|
|
if len(out) >= max_count:
|
|
break
|
|
return out
|
|
|
|
|
|
def mst_edges_for_link(markers: List[Dict[str, Any]]) -> List[Tuple[int, int]]:
|
|
n = len(markers)
|
|
if n < 2:
|
|
return []
|
|
|
|
ids = [int(m["id"]) for m in markers]
|
|
pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0)
|
|
in_tree = np.zeros(n, dtype=bool)
|
|
in_tree[0] = True
|
|
edges: List[Tuple[int, int]] = []
|
|
dist = np.linalg.norm(pos[:, None, :] - pos[None, :, :], axis=2)
|
|
|
|
for _ in range(n - 1):
|
|
best = None
|
|
best_d = float("inf")
|
|
for i in range(n):
|
|
if not in_tree[i]:
|
|
continue
|
|
for j in range(n):
|
|
if in_tree[j]:
|
|
continue
|
|
d = float(dist[i, j])
|
|
if d < best_d:
|
|
best_d = d
|
|
best = (i, j)
|
|
if best is None:
|
|
break
|
|
i, j = best
|
|
in_tree[j] = True
|
|
edges.append((ids[i], ids[j]))
|
|
return edges
|
|
|
|
|
|
def compile_rigid_distance_constraints(
|
|
robot_data: Dict[str, Any],
|
|
link_markers: Dict[str, List[Dict[str, Any]]],
|
|
cfg: ConstraintRuleConfig
|
|
) -> List[MarkerDistanceConstraint]:
|
|
constraints: List[MarkerDistanceConstraint] = []
|
|
|
|
for link_name, markers in link_markers.items():
|
|
if not get_enabled_link_rule(robot_data, link_name, "rigid_distance", cfg.rigid_distance_enabled):
|
|
continue
|
|
if len(markers) < 2:
|
|
continue
|
|
|
|
mode = cfg.rigid_distance_mode
|
|
if mode == "full":
|
|
pairs = [(int(a["id"]), int(b["id"])) for a, b in combinations(markers, 2)]
|
|
elif mode == "star":
|
|
anchor_ids = select_anchor_marker_ids(markers, axis=None, max_count=1)
|
|
anchor_id = anchor_ids[0]
|
|
pairs = []
|
|
for m in markers:
|
|
mid = int(m["id"])
|
|
if mid != anchor_id:
|
|
pairs.append((anchor_id, mid))
|
|
elif mode == "mst":
|
|
pairs = mst_edges_for_link(markers)
|
|
else:
|
|
raise ValueError(f"Unknown rigid_distance_mode='{mode}'. Use mst|star|full.")
|
|
|
|
pos_map = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in markers}
|
|
seen_pairs: set[Tuple[int, int]] = set()
|
|
|
|
for mid_a, mid_b in pairs:
|
|
if mid_a == mid_b:
|
|
continue
|
|
key = tuple(sorted((int(mid_a), int(mid_b))))
|
|
if key in seen_pairs:
|
|
continue
|
|
seen_pairs.add(key)
|
|
|
|
pos_a = pos_map[mid_a]
|
|
pos_b = pos_map[mid_b]
|
|
target = float(np.linalg.norm(pos_b - pos_a))
|
|
|
|
constraints.append(
|
|
MarkerDistanceConstraint(
|
|
marker_id_a=mid_a,
|
|
marker_id_b=mid_b,
|
|
link_name=link_name,
|
|
target_distance_m=target,
|
|
weight=cfg.rigid_distance_weight,
|
|
enabled=True,
|
|
source=f"rigid_distance:{mode}",
|
|
)
|
|
)
|
|
|
|
return constraints
|
|
|
|
|
|
def compile_direct_joint_axis_constraints(
|
|
robot_data: Dict[str, Any],
|
|
link_markers: Dict[str, List[Dict[str, Any]]],
|
|
cfg: ConstraintRuleConfig
|
|
) -> List[JointAxisConstraint]:
|
|
constraints: List[JointAxisConstraint] = []
|
|
links = robot_data.get("links", {}) or {}
|
|
|
|
for child_link, child_data in links.items():
|
|
parent_link = child_data.get("parent", None)
|
|
if not parent_link:
|
|
continue
|
|
|
|
if not get_enabled_link_rule(robot_data, child_link, "joint_axis_projection", cfg.joint_axis_enabled):
|
|
continue
|
|
|
|
joint_axis = get_joint_axis(robot_data, child_link)
|
|
if joint_axis is None:
|
|
continue
|
|
|
|
axis_vec = principal_axis_vector(joint_axis)
|
|
|
|
parent_markers = link_markers.get(parent_link, [])
|
|
child_markers = link_markers.get(child_link, [])
|
|
if len(parent_markers) == 0 or len(child_markers) == 0:
|
|
continue
|
|
|
|
max_pairs = max(1, int(cfg.joint_axis_max_pairs))
|
|
parent_anchor_ids = select_anchor_marker_ids(parent_markers, axis=axis_vec, max_count=max_pairs)
|
|
child_anchor_ids = select_anchor_marker_ids(child_markers, axis=axis_vec, max_count=max_pairs)
|
|
|
|
parent_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in parent_markers}
|
|
child_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in child_markers}
|
|
|
|
seen: set[Tuple[int, int]] = set()
|
|
for mid_p in parent_anchor_ids:
|
|
for mid_c in child_anchor_ids:
|
|
if mid_p == mid_c:
|
|
continue
|
|
key = (mid_p, mid_c)
|
|
if key in seen:
|
|
continue
|
|
seen.add(key)
|
|
|
|
delta = child_pos[mid_c] - parent_pos[mid_p]
|
|
target = float(np.dot(delta, axis_vec))
|
|
|
|
constraints.append(
|
|
JointAxisConstraint(
|
|
marker_id_parent=mid_p,
|
|
marker_id_child=mid_c,
|
|
parent_link=parent_link,
|
|
child_link=child_link,
|
|
joint_axis=axis_vec,
|
|
target_delta_along_axis_m=target,
|
|
weight=cfg.joint_axis_weight,
|
|
enabled=True,
|
|
source="joint_axis_projection",
|
|
)
|
|
)
|
|
|
|
return constraints
|
|
|
|
|
|
def ancestors_of(link_name: str, parent_map: Dict[str, Optional[str]], max_depth: int) -> List[str]:
|
|
out = []
|
|
cur = parent_map.get(link_name, None)
|
|
depth = 0
|
|
while cur is not None and depth < max_depth:
|
|
out.append(cur)
|
|
cur = parent_map.get(cur, None)
|
|
depth += 1
|
|
return out
|
|
|
|
|
|
def path_axes_consistent(
|
|
robot_data: Dict[str, Any],
|
|
ancestor: str,
|
|
descendant: str,
|
|
parent_map: Dict[str, Optional[str]],
|
|
threshold: float
|
|
) -> Optional[np.ndarray]:
|
|
chain: List[str] = []
|
|
cur = descendant
|
|
while cur is not None and cur != ancestor:
|
|
chain.append(cur)
|
|
cur = parent_map.get(cur, None)
|
|
|
|
if cur != ancestor:
|
|
return None
|
|
|
|
chain.reverse()
|
|
axes: List[np.ndarray] = []
|
|
for link in chain:
|
|
ax = get_joint_axis(robot_data, link)
|
|
if ax is None:
|
|
return None
|
|
axes.append(ax)
|
|
|
|
if not axes:
|
|
return None
|
|
|
|
axis_ids: List[int] = []
|
|
signs: List[float] = []
|
|
for ax in axes:
|
|
aid = principal_axis_id(ax, threshold=threshold)
|
|
if aid is None:
|
|
return None
|
|
axis_ids.append(aid)
|
|
signs.append(float(np.sign(ax[aid])) if abs(ax[aid]) > 1e-12 else 1.0)
|
|
|
|
if len(set(axis_ids)) != 1:
|
|
return None
|
|
|
|
axis_id = axis_ids[0]
|
|
sign = 1.0 if np.sum(signs) >= 0 else -1.0
|
|
axis_vec = np.zeros(3, dtype=np.float64)
|
|
axis_vec[axis_id] = sign
|
|
return normalize_vector(axis_vec)
|
|
|
|
|
|
def compile_chain_axis_constraints(
|
|
robot_data: Dict[str, Any],
|
|
link_markers: Dict[str, List[Dict[str, Any]]],
|
|
cfg: ConstraintRuleConfig
|
|
) -> List[JointAxisConstraint]:
|
|
constraints: List[JointAxisConstraint] = []
|
|
parent_map = get_link_parent_map(robot_data)
|
|
|
|
links_with_markers = [ln for ln, mk in link_markers.items() if len(mk) > 0]
|
|
max_depth = max(1, int(cfg.chain_axis_max_depth))
|
|
max_pairs = max(1, int(cfg.chain_axis_max_pairs))
|
|
|
|
pos_cache: Dict[int, np.ndarray] = {}
|
|
for mk in link_markers.values():
|
|
for m in mk:
|
|
pos_cache[int(m["id"])] = np.asarray(m["position_m"], dtype=np.float64)
|
|
|
|
for descendant in links_with_markers:
|
|
if not get_enabled_link_rule(robot_data, descendant, "chain_axis_projection", cfg.chain_axis_enabled):
|
|
continue
|
|
|
|
for ancestor in ancestors_of(descendant, parent_map, max_depth=max_depth):
|
|
if ancestor == descendant:
|
|
continue
|
|
|
|
axis_vec = path_axes_consistent(
|
|
robot_data=robot_data,
|
|
ancestor=ancestor,
|
|
descendant=descendant,
|
|
parent_map=parent_map,
|
|
threshold=cfg.axis_alignment_threshold,
|
|
)
|
|
if axis_vec is None:
|
|
continue
|
|
|
|
ancestor_markers = link_markers.get(ancestor, [])
|
|
descendant_markers = link_markers.get(descendant, [])
|
|
if len(ancestor_markers) == 0 or len(descendant_markers) == 0:
|
|
continue
|
|
|
|
anc_anchors = select_anchor_marker_ids(ancestor_markers, axis=axis_vec, max_count=max_pairs)
|
|
des_anchors = select_anchor_marker_ids(descendant_markers, axis=axis_vec, max_count=max_pairs)
|
|
|
|
seen: set[Tuple[int, int]] = set()
|
|
for mid_a in anc_anchors:
|
|
for mid_b in des_anchors:
|
|
if mid_a == mid_b:
|
|
continue
|
|
key = (mid_a, mid_b)
|
|
if key in seen:
|
|
continue
|
|
seen.add(key)
|
|
|
|
target = float(np.dot(pos_cache[mid_b] - pos_cache[mid_a], axis_vec))
|
|
constraints.append(
|
|
JointAxisConstraint(
|
|
marker_id_parent=mid_a,
|
|
marker_id_child=mid_b,
|
|
parent_link=ancestor,
|
|
child_link=descendant,
|
|
joint_axis=axis_vec,
|
|
target_delta_along_axis_m=target,
|
|
weight=cfg.chain_axis_weight,
|
|
enabled=True,
|
|
source=f"chain_axis_projection:depth{len(ancestors_of(descendant, parent_map, max_depth))}",
|
|
)
|
|
)
|
|
|
|
return constraints
|
|
|
|
|
|
def compile_constraints(
|
|
robot_data: Dict[str, Any],
|
|
length_scale: float,
|
|
cfg: ConstraintRuleConfig
|
|
) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[Constraint], List[str], Dict[int, Dict[str, Any]]]:
|
|
marker_to_link, link_markers, issues, marker_meta = parse_robot_markers(
|
|
robot_data,
|
|
length_scale=length_scale,
|
|
strict_unique_marker_ids=cfg.strict_unique_marker_ids,
|
|
)
|
|
|
|
constraints: List[Constraint] = []
|
|
constraints.extend(compile_rigid_distance_constraints(robot_data, link_markers, cfg))
|
|
constraints.extend(compile_direct_joint_axis_constraints(robot_data, link_markers, cfg))
|
|
constraints.extend(compile_chain_axis_constraints(robot_data, link_markers, cfg))
|
|
|
|
unique_constraints: List[Constraint] = []
|
|
seen_keys: set[Tuple[Any, ...]] = set()
|
|
|
|
for c in constraints:
|
|
if isinstance(c, MarkerDistanceConstraint):
|
|
key = (
|
|
"d",
|
|
min(c.marker_id_a, c.marker_id_b),
|
|
max(c.marker_id_a, c.marker_id_b),
|
|
c.link_name,
|
|
round(c.target_distance_m, 9),
|
|
)
|
|
else:
|
|
key = (
|
|
"j",
|
|
c.parent_link,
|
|
c.child_link,
|
|
c.marker_id_parent,
|
|
c.marker_id_child,
|
|
tuple(np.round(c.joint_axis, 9).tolist()),
|
|
round(c.target_delta_along_axis_m, 9),
|
|
)
|
|
if key in seen_keys:
|
|
continue
|
|
seen_keys.add(key)
|
|
unique_constraints.append(c)
|
|
|
|
return marker_to_link, link_markers, unique_constraints, issues, marker_meta
|
|
|
|
|
|
# ===================================================================
|
|
# Observation quality / weighting
|
|
# ===================================================================
|
|
|
|
def _optional_float(meta: Dict[str, Any], keys: List[str]) -> Optional[float]:
|
|
for k in keys:
|
|
if k in meta and meta[k] is not None:
|
|
try:
|
|
return float(meta[k])
|
|
except Exception:
|
|
pass
|
|
return None
|
|
|
|
|
|
def detection_quality_from_metadata(det_obj: Dict[str, Any], cfg: ConstraintRuleConfig) -> float:
|
|
q = 1.0
|
|
|
|
if cfg.use_detection_confidence:
|
|
conf = _optional_float(det_obj, ["confidence", "score", "quality", "det_confidence"])
|
|
if conf is not None:
|
|
q *= clamp(conf, 0.1, 1.0)
|
|
|
|
if cfg.use_detection_size_px:
|
|
size_px = _optional_float(det_obj, ["size_px", "marker_size_px", "side_px", "side_length_px"])
|
|
if size_px is None and "corners_px" in det_obj and isinstance(det_obj["corners_px"], list):
|
|
try:
|
|
corners = np.asarray(det_obj["corners_px"], dtype=np.float64).reshape(-1, 2)
|
|
if len(corners) >= 4:
|
|
edges = []
|
|
for i in range(len(corners)):
|
|
p = corners[i]
|
|
q2 = corners[(i + 1) % len(corners)]
|
|
edges.append(float(np.linalg.norm(q2 - p)))
|
|
size_px = float(np.mean(edges))
|
|
except Exception:
|
|
size_px = None
|
|
if size_px is not None:
|
|
q *= clamp(size_px / max(cfg.ref_marker_size_px, 1e-6), 0.25, 3.0)
|
|
|
|
sharpness = _optional_float(det_obj, ["sharpness", "corner_sharpness"])
|
|
if sharpness is not None:
|
|
q *= clamp(sharpness / 2500.0, 0.5, 1.5)
|
|
|
|
normal_alignment = _optional_float(det_obj, ["normal_alignment", "view_cosine", "cos_to_camera"])
|
|
if normal_alignment is not None:
|
|
q *= clamp(normal_alignment, 0.3, 1.0)
|
|
|
|
return float(q)
|
|
|
|
|
|
def marker_size_prior_factor(marker_meta: Dict[str, Any], default_marker_size_m: float) -> float:
|
|
size_val = marker_meta.get("size", None)
|
|
if size_val is None:
|
|
return 1.0
|
|
|
|
try:
|
|
size_val = float(size_val)
|
|
except Exception:
|
|
return 1.0
|
|
|
|
size_m = size_val / 1000.0 if size_val > 1.0 else size_val
|
|
ref = max(default_marker_size_m, 1e-6)
|
|
return clamp(size_m / ref, 0.7, 1.3)
|
|
|
|
|
|
def compute_observation_weights(
|
|
marker_observations: Dict[int, List[Observation]],
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]],
|
|
initial_positions: Dict[int, np.ndarray],
|
|
marker_meta: Dict[int, Dict[str, Any]],
|
|
cfg: ConstraintRuleConfig,
|
|
robot_data: Dict[str, Any]
|
|
) -> Dict[Tuple[int, int], float]:
|
|
weights: Dict[Tuple[int, int], float] = {}
|
|
default_marker_size_m = get_vision_marker_size_default(robot_data)
|
|
|
|
for marker_id, obs_list in marker_observations.items():
|
|
X = initial_positions.get(marker_id, None)
|
|
size_prior = marker_size_prior_factor(marker_meta.get(marker_id, {}), default_marker_size_m)
|
|
|
|
for obs_idx, obs in enumerate(obs_list):
|
|
w = 1.0
|
|
q = detection_quality_from_metadata(obs.meta, cfg)
|
|
w *= q
|
|
|
|
if cfg.use_marker_size_prior:
|
|
w *= size_prior
|
|
|
|
if cfg.use_initial_range and X is not None:
|
|
_, _, R_wc, t_wc = cameras[obs.cam_idx]
|
|
C = camera_center_from_world_to_cam(R_wc, t_wc)
|
|
dist = float(np.linalg.norm(X - C))
|
|
if np.isfinite(dist):
|
|
w *= clamp(cfg.ref_distance_m / max(dist, 1e-6), 0.4, 2.0)
|
|
|
|
weights[(marker_id, obs_idx)] = clamp(w, cfg.weight_floor, cfg.weight_ceiling)
|
|
|
|
return weights
|
|
|
|
|
|
# ===================================================================
|
|
# Multi-view loading
|
|
# ===================================================================
|
|
|
|
def load_observations_and_poses(
|
|
detection_files: List[str],
|
|
pose_files: List[str]
|
|
) -> Tuple[
|
|
Dict[int, List[Observation]],
|
|
List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]],
|
|
List[Dict[str, Any]]
|
|
]:
|
|
if len(detection_files) != len(pose_files):
|
|
raise ValueError(f"Mismatch: {len(detection_files)} detections vs {len(pose_files)} poses")
|
|
|
|
marker_observations: Dict[int, List[Observation]] = {}
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
|
|
obs_metadata: List[Dict[str, Any]] = []
|
|
|
|
for cam_idx, (det_file, pose_file) in enumerate(zip(detection_files, pose_files)):
|
|
det = load_json(det_file)
|
|
pose_data = load_json(pose_file)
|
|
|
|
cam_section = det.get("camera", {}) or {}
|
|
K = np.array(cam_section.get("camera_matrix", []), dtype=np.float64).reshape(3, 3)
|
|
D = np.array(cam_section.get("distortion_coefficients", []), dtype=np.float64).reshape(-1, 1)
|
|
|
|
pose_section = pose_data.get("camera_pose", {}) or {}
|
|
world_to_cam = pose_section.get("world_to_camera", {}) or {}
|
|
R_wc = np.array(world_to_cam.get("rotation_matrix", []), dtype=np.float64).reshape(3, 3)
|
|
t_wc = np.array(world_to_cam.get("translation_m", []), dtype=np.float64).reshape(3)
|
|
|
|
cameras.append((K, D, R_wc, t_wc))
|
|
|
|
detections = det.get("detections", []) or []
|
|
for det_obj in detections:
|
|
marker_id = int(det_obj.get("marker_id", -1))
|
|
if marker_id < 0:
|
|
continue
|
|
|
|
center_px = np.array(det_obj.get("center_px", []), dtype=np.float64)
|
|
if center_px.shape != (2,):
|
|
continue
|
|
|
|
pts = center_px.reshape(1, 1, 2).astype(np.float32)
|
|
und = cv2.undistortPoints(pts, K.astype(np.float32), D.astype(np.float32), P=None)
|
|
norm_coords = und.reshape(2).astype(np.float64)
|
|
|
|
obs = Observation(cam_idx=cam_idx, norm_coords=norm_coords, meta=dict(det_obj))
|
|
marker_observations.setdefault(marker_id, []).append(obs)
|
|
|
|
obs_metadata.append(
|
|
{
|
|
"detection_file": det_file,
|
|
"pose_file": pose_file,
|
|
"num_detections": len(detections),
|
|
}
|
|
)
|
|
|
|
return marker_observations, cameras, obs_metadata
|
|
|
|
|
|
# ===================================================================
|
|
# Initial triangulation
|
|
# ===================================================================
|
|
|
|
def triangulate_marker_initial(
|
|
marker_id: int,
|
|
observations: List[Observation],
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]]
|
|
) -> Optional[np.ndarray]:
|
|
if len(observations) < 2:
|
|
return None
|
|
|
|
best_pair = None
|
|
best_baseline = -1.0
|
|
|
|
for obs_i, obs_j in combinations(observations, 2):
|
|
cam_i, cam_j = obs_i.cam_idx, obs_j.cam_idx
|
|
_, _, R1, t1 = cameras[cam_i]
|
|
_, _, R2, t2 = cameras[cam_j]
|
|
c1 = camera_center_from_world_to_cam(R1, t1)
|
|
c2 = camera_center_from_world_to_cam(R2, t2)
|
|
baseline = float(np.linalg.norm(c2 - c1))
|
|
if baseline > best_baseline:
|
|
best_baseline = baseline
|
|
best_pair = (obs_i, obs_j)
|
|
|
|
if best_pair is None:
|
|
return None
|
|
|
|
obs_i, obs_j = best_pair
|
|
cam_i, cam_j = obs_i.cam_idx, obs_j.cam_idx
|
|
norm_coords_i = obs_i.norm_coords
|
|
norm_coords_j = obs_j.norm_coords
|
|
|
|
K1, D1, R1, t1 = cameras[cam_i]
|
|
K2, D2, R2, t2 = cameras[cam_j]
|
|
|
|
x1_px = K1[0, 0] * norm_coords_i[0] + K1[0, 2]
|
|
y1_px = K1[1, 1] * norm_coords_i[1] + K1[1, 2]
|
|
x2_px = K2[0, 0] * norm_coords_j[0] + K2[0, 2]
|
|
y2_px = K2[1, 1] * norm_coords_j[1] + K2[1, 2]
|
|
|
|
P1 = K1 @ np.hstack([R1, t1.reshape(3, 1)])
|
|
P2 = K2 @ np.hstack([R2, t2.reshape(3, 1)])
|
|
|
|
try:
|
|
X_h = cv2.triangulatePoints(
|
|
P1,
|
|
P2,
|
|
np.array([[x1_px], [y1_px]], dtype=np.float64),
|
|
np.array([[x2_px], [y2_px]], dtype=np.float64),
|
|
)
|
|
X = (X_h[:3] / X_h[3]).reshape(3).astype(np.float64)
|
|
if not np.all(np.isfinite(X)):
|
|
return None
|
|
return X
|
|
except Exception:
|
|
return None
|
|
|
|
|
|
def initial_triangulation(
|
|
marker_observations: Dict[int, List[Observation]],
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]]
|
|
) -> Dict[int, np.ndarray]:
|
|
triangulated: Dict[int, np.ndarray] = {}
|
|
for marker_id, observations in marker_observations.items():
|
|
X = triangulate_marker_initial(marker_id, observations, cameras)
|
|
if X is not None:
|
|
triangulated[marker_id] = X
|
|
return triangulated
|
|
|
|
|
|
# ===================================================================
|
|
# Weighted residuals / optimization
|
|
# ===================================================================
|
|
|
|
def bundle_adjustment_residuals(
|
|
marker_positions_flat: np.ndarray,
|
|
marker_ids: List[int],
|
|
marker_observations: Dict[int, List[Observation]],
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]],
|
|
constraints: List[Constraint],
|
|
obs_weights: Dict[Tuple[int, int], float],
|
|
lambda_constraint: float = 100.0
|
|
) -> np.ndarray:
|
|
marker_dict: Dict[int, np.ndarray] = {}
|
|
for i, marker_id in enumerate(marker_ids):
|
|
marker_dict[marker_id] = marker_positions_flat[i * 3:(i + 1) * 3]
|
|
|
|
residuals: List[float] = []
|
|
|
|
for marker_id, observations in marker_observations.items():
|
|
if marker_id not in marker_dict:
|
|
continue
|
|
|
|
X_world = marker_dict[marker_id]
|
|
for obs_idx, obs in enumerate(observations):
|
|
cam_idx, norm_coords_obs = obs.cam_idx, obs.norm_coords
|
|
K, D, R_wc, t_wc = cameras[cam_idx]
|
|
X_cam = R_wc @ X_world + t_wc
|
|
if X_cam[2] > 1e-6:
|
|
proj_norm = X_cam[:2] / X_cam[2]
|
|
r = proj_norm - norm_coords_obs
|
|
w = float(np.sqrt(obs_weights.get((marker_id, obs_idx), 1.0)))
|
|
residuals.append(w * float(r[0]))
|
|
residuals.append(w * float(r[1]))
|
|
|
|
for constraint in constraints:
|
|
if isinstance(constraint, MarkerDistanceConstraint):
|
|
if constraint.marker_id_a in marker_dict and constraint.marker_id_b in marker_dict:
|
|
pos_a = marker_dict[constraint.marker_id_a]
|
|
pos_b = marker_dict[constraint.marker_id_b]
|
|
actual_dist = float(np.linalg.norm(pos_b - pos_a))
|
|
residuals.append((actual_dist - constraint.target_distance_m) * constraint.weight * lambda_constraint)
|
|
|
|
elif isinstance(constraint, JointAxisConstraint):
|
|
if constraint.marker_id_parent in marker_dict and constraint.marker_id_child in marker_dict:
|
|
pos_parent = marker_dict[constraint.marker_id_parent]
|
|
pos_child = marker_dict[constraint.marker_id_child]
|
|
delta = pos_child - pos_parent
|
|
actual_delta = float(np.dot(delta, constraint.joint_axis))
|
|
residuals.append((actual_delta - constraint.target_delta_along_axis_m) * constraint.weight * lambda_constraint)
|
|
|
|
return np.asarray(residuals, dtype=np.float64)
|
|
|
|
|
|
def optimize_with_constraints(
|
|
initial_positions: Dict[int, np.ndarray],
|
|
marker_observations: Dict[int, List[Observation]],
|
|
cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]],
|
|
constraints: List[Constraint],
|
|
obs_weights: Dict[Tuple[int, int], float],
|
|
lambda_constraint: float = 100.0,
|
|
max_iterations: int = 50
|
|
) -> Dict[int, np.ndarray]:
|
|
try:
|
|
from scipy.optimize import least_squares
|
|
except ImportError:
|
|
print("[WARN] scipy not available, skipping optimization.")
|
|
return initial_positions
|
|
|
|
marker_ids = sorted(initial_positions.keys())
|
|
if not marker_ids:
|
|
return {}
|
|
|
|
x0 = np.concatenate([initial_positions[mid] for mid in marker_ids])
|
|
|
|
def residuals_fn(x: np.ndarray) -> np.ndarray:
|
|
return bundle_adjustment_residuals(
|
|
x, marker_ids, marker_observations, cameras, constraints, obs_weights, lambda_constraint
|
|
)
|
|
|
|
print(f"[INFO] Starting optimization with {len(x0)} variables and {len(constraints)} constraints...")
|
|
|
|
result = least_squares(
|
|
residuals_fn,
|
|
x0,
|
|
max_nfev=max_iterations * max(1, len(marker_ids)),
|
|
verbose=1,
|
|
)
|
|
|
|
optimized = {}
|
|
for i, marker_id in enumerate(marker_ids):
|
|
optimized[marker_id] = result.x[i * 3:(i + 1) * 3]
|
|
|
|
print(f"[INFO] Optimization complete. Final cost: {float(np.sum(result.fun ** 2)):.6f}")
|
|
return optimized
|
|
|
|
|
|
# ===================================================================
|
|
# Reporting helpers
|
|
# ===================================================================
|
|
|
|
def print_constraint_summary(constraints: List[Constraint]) -> None:
|
|
num_dist = sum(isinstance(c, MarkerDistanceConstraint) for c in constraints)
|
|
num_joint = sum(isinstance(c, JointAxisConstraint) for c in constraints)
|
|
print(f"[INFO] Constraint summary: total={len(constraints)} distance={num_dist} joint/chain={num_joint}")
|
|
|
|
|
|
def print_constraint_list(constraints: List[Constraint]) -> None:
|
|
print("\n[INFO] Constraint list:")
|
|
for idx, constraint in enumerate(constraints):
|
|
if isinstance(constraint, MarkerDistanceConstraint):
|
|
print(
|
|
f" [{idx:03d}] DISTANCE | "
|
|
f"Link='{constraint.link_name}' | "
|
|
f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | "
|
|
f"Target={constraint.target_distance_m:.6f} m | "
|
|
f"Weight={constraint.weight} | "
|
|
f"Source={constraint.source}"
|
|
)
|
|
else:
|
|
axis_str = np.array2string(constraint.joint_axis, precision=3, suppress_small=True)
|
|
print(
|
|
f" [{idx:03d}] JOINT_AXIS | "
|
|
f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> "
|
|
f"{constraint.child_link}(M{constraint.marker_id_child}) | "
|
|
f"Axis={axis_str} | "
|
|
f"TargetDelta={constraint.target_delta_along_axis_m:.6f} m | "
|
|
f"Weight={constraint.weight} | "
|
|
f"Source={constraint.source}"
|
|
)
|
|
|
|
|
|
def print_constraints_with_errors(
|
|
title: str,
|
|
constraints: List[Constraint],
|
|
positions: Dict[int, np.ndarray],
|
|
show_skipped: bool = True
|
|
) -> None:
|
|
print(f"\n[INFO] {title}")
|
|
|
|
active = 0
|
|
skipped = 0
|
|
|
|
for idx, constraint in enumerate(constraints):
|
|
if isinstance(constraint, MarkerDistanceConstraint):
|
|
if constraint.marker_id_a not in positions or constraint.marker_id_b not in positions:
|
|
skipped += 1
|
|
if show_skipped:
|
|
print(
|
|
f" [{idx:03d}] DISTANCE | "
|
|
f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | SKIPPED (missing marker)"
|
|
)
|
|
continue
|
|
|
|
pos_a = positions[constraint.marker_id_a]
|
|
pos_b = positions[constraint.marker_id_b]
|
|
actual = float(np.linalg.norm(pos_b - pos_a))
|
|
error = actual - constraint.target_distance_m
|
|
active += 1
|
|
|
|
print(
|
|
f" [{idx:03d}] DISTANCE | "
|
|
f"Link='{constraint.link_name}' | "
|
|
f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | "
|
|
f"target={constraint.target_distance_m*1000:.2f} mm | "
|
|
f"actual={actual*1000:.2f} mm | "
|
|
f"error={error*1000:+.2f} mm"
|
|
)
|
|
|
|
elif isinstance(constraint, JointAxisConstraint):
|
|
if constraint.marker_id_parent not in positions or constraint.marker_id_child not in positions:
|
|
skipped += 1
|
|
if show_skipped:
|
|
print(
|
|
f" [{idx:03d}] JOINT_AXIS | "
|
|
f"M{constraint.marker_id_parent} -> M{constraint.marker_id_child} | SKIPPED (missing marker)"
|
|
)
|
|
continue
|
|
|
|
pos_parent = positions[constraint.marker_id_parent]
|
|
pos_child = positions[constraint.marker_id_child]
|
|
delta = pos_child - pos_parent
|
|
actual = float(np.dot(delta, constraint.joint_axis))
|
|
error = actual - constraint.target_delta_along_axis_m
|
|
active += 1
|
|
|
|
axis_str = np.array2string(constraint.joint_axis, precision=2, suppress_small=True)
|
|
print(
|
|
f" [{idx:03d}] JOINT_AXIS | "
|
|
f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> "
|
|
f"{constraint.child_link}(M{constraint.marker_id_child}) | "
|
|
f"axis={axis_str} | "
|
|
f"target={constraint.target_delta_along_axis_m*1000:.2f} mm | "
|
|
f"actual={actual*1000:.2f} mm | "
|
|
f"error={error*1000:+.2f} mm"
|
|
)
|
|
|
|
print(f"[INFO] Active constraints: {active} | Skipped: {skipped}")
|
|
|
|
|
|
def print_observation_weight_summary(obs_weights: Dict[Tuple[int, int], float]) -> None:
|
|
if not obs_weights:
|
|
print("[INFO] Observation weighting: disabled or empty")
|
|
return
|
|
values = np.array(list(obs_weights.values()), dtype=np.float64)
|
|
print(
|
|
"[INFO] Observation weights: "
|
|
f"min={values.min():.3f} mean={values.mean():.3f} "
|
|
f"median={np.median(values):.3f} max={values.max():.3f}"
|
|
)
|
|
|
|
|
|
# ===================================================================
|
|
# Main
|
|
# ===================================================================
|
|
|
|
def main() -> None:
|
|
parser = argparse.ArgumentParser(
|
|
description="Multi-view bundle adjustment with rule-based geometric constraints"
|
|
)
|
|
parser.add_argument(
|
|
"-det", "--detections",
|
|
action="append",
|
|
required=True,
|
|
help="*_aruco_detection.json files"
|
|
)
|
|
parser.add_argument(
|
|
"-pose", "--poses",
|
|
action="append",
|
|
required=True,
|
|
help="*_camera_pose.json files"
|
|
)
|
|
parser.add_argument(
|
|
"-robot", "--robot",
|
|
required=True,
|
|
help="robot.json"
|
|
)
|
|
parser.add_argument(
|
|
"-outDir", "--outDir",
|
|
default=None,
|
|
help="Output directory"
|
|
)
|
|
parser.add_argument(
|
|
"-lambdaWeight", "--lambdaWeight",
|
|
type=float,
|
|
default=100.0,
|
|
help="Constraint weight multiplier"
|
|
)
|
|
parser.add_argument(
|
|
"--strictUniqueMarkerIds",
|
|
action="store_true",
|
|
help="Fail if a marker ID appears more than once in robot.json"
|
|
)
|
|
parser.add_argument(
|
|
"--showSkippedConstraints",
|
|
action="store_true",
|
|
help="Print skipped constraints in the report"
|
|
)
|
|
parser.add_argument(
|
|
"--noShowSkippedConstraints",
|
|
action="store_true",
|
|
help="Hide skipped constraints in the report"
|
|
)
|
|
parser.add_argument(
|
|
"--saveConstraintReport",
|
|
action="store_true",
|
|
help="Save constraint report JSON files"
|
|
)
|
|
parser.add_argument(
|
|
"--saveObservationWeightReport",
|
|
action="store_true",
|
|
help="Save observation-weight report JSON file"
|
|
)
|
|
|
|
args = parser.parse_args()
|
|
|
|
if args.showSkippedConstraints and args.noShowSkippedConstraints:
|
|
print("[ERROR] Choose only one of --showSkippedConstraints or --noShowSkippedConstraints")
|
|
sys.exit(1)
|
|
|
|
if len(args.detections) != len(args.poses):
|
|
print(f"[ERROR] Mismatch: {len(args.detections)} detection files vs {len(args.poses)} pose files")
|
|
sys.exit(1)
|
|
|
|
robot_data = load_json(args.robot)
|
|
length_scale = get_length_scale(robot_data)
|
|
cfg = load_constraint_rule_config(robot_data, args)
|
|
|
|
print("[STEP 1] Compile constraints from robot.json structure...")
|
|
marker_to_link, link_markers, constraints, issues, marker_meta = compile_constraints(robot_data, length_scale, cfg)
|
|
|
|
for issue in issues:
|
|
print(issue)
|
|
|
|
print(f"[INFO] Links with markers: {sum(1 for v in link_markers.values() if len(v) > 0)}")
|
|
print(f"[INFO] Unique marker IDs: {len(marker_to_link)}")
|
|
print_constraint_summary(constraints)
|
|
print_constraint_list(constraints)
|
|
|
|
print("\n[STEP 2] Load observations and camera poses...")
|
|
marker_observations, cameras, obs_metadata = load_observations_and_poses(args.detections, args.poses)
|
|
print(f"[INFO] {len(cameras)} cameras, {len(marker_observations)} observed markers")
|
|
print(f"[INFO] Detection files loaded: {len(obs_metadata)}")
|
|
|
|
print("\n[STEP 3] Initial triangulation...")
|
|
initial_pos = initial_triangulation(marker_observations, cameras)
|
|
print(f"[INFO] Triangulated {len(initial_pos)} markers")
|
|
|
|
out_dir = args.outDir or os.path.dirname(args.detections[0]) or "."
|
|
os.makedirs(resolve_path(out_dir), exist_ok=True)
|
|
|
|
initial_output_markers = []
|
|
for marker_id, position in sorted(initial_pos.items()):
|
|
initial_output_markers.append(
|
|
{
|
|
"marker_id": int(marker_id),
|
|
"position_m": [float(x) for x in position],
|
|
"position_mm": [float(x * 1000.0) for x in position],
|
|
"link": marker_to_link.get(marker_id, "unknown"),
|
|
}
|
|
)
|
|
|
|
initial_output = {
|
|
"schema_version": "1.2",
|
|
"stage": "initial_triangulation",
|
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
|
"summary": {
|
|
"num_cameras": len(cameras),
|
|
"num_markers": len(initial_pos),
|
|
"num_constraints": len(constraints),
|
|
},
|
|
"markers": initial_output_markers,
|
|
}
|
|
initial_out_file = os.path.join(out_dir, "aruco_positions_initial.json")
|
|
save_json(initial_out_file, initial_output)
|
|
print(f"[INFO] Initial triangulation saved to {initial_out_file}")
|
|
|
|
obs_weights = compute_observation_weights(
|
|
marker_observations=marker_observations,
|
|
cameras=cameras,
|
|
initial_positions=initial_pos,
|
|
marker_meta=marker_meta,
|
|
cfg=cfg,
|
|
robot_data=robot_data,
|
|
)
|
|
print_observation_weight_summary(obs_weights)
|
|
|
|
print_constraints_with_errors(
|
|
"Constraint list BEFORE optimization",
|
|
constraints,
|
|
initial_pos,
|
|
show_skipped=cfg.show_skipped_constraints,
|
|
)
|
|
|
|
print("\n[STEP 4] Bundle adjustment with constraints...")
|
|
optimized_pos = optimize_with_constraints(
|
|
initial_pos,
|
|
marker_observations,
|
|
cameras,
|
|
constraints,
|
|
obs_weights,
|
|
lambda_constraint=args.lambdaWeight,
|
|
)
|
|
|
|
print_constraints_with_errors(
|
|
"Constraint list AFTER optimization",
|
|
constraints,
|
|
optimized_pos,
|
|
show_skipped=cfg.show_skipped_constraints,
|
|
)
|
|
|
|
output_markers = []
|
|
for marker_id, position in sorted(optimized_pos.items()):
|
|
output_markers.append(
|
|
{
|
|
"marker_id": int(marker_id),
|
|
"position_m": [float(x) for x in position],
|
|
"position_mm": [float(x * 1000.0) for x in position],
|
|
"link": marker_to_link.get(marker_id, "unknown"),
|
|
}
|
|
)
|
|
|
|
output = {
|
|
"schema_version": "1.2",
|
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
|
"summary": {
|
|
"num_cameras": len(cameras),
|
|
"num_markers": len(optimized_pos),
|
|
"num_constraints": len(constraints),
|
|
},
|
|
"markers": output_markers,
|
|
}
|
|
out_file = os.path.join(out_dir, "aruco_positions_optimized.json")
|
|
save_json(out_file, output)
|
|
print(f"\n[INFO] Saved to {out_file}")
|
|
|
|
if args.saveConstraintReport:
|
|
report = {
|
|
"schema_version": "1.0",
|
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
|
"summary": {
|
|
"num_constraints": len(constraints),
|
|
"num_links_with_markers": sum(1 for v in link_markers.values() if len(v) > 0),
|
|
"num_observed_markers": len(marker_observations),
|
|
"num_triangulated_markers": len(initial_pos),
|
|
"num_optimized_markers": len(optimized_pos),
|
|
},
|
|
"constraints": [],
|
|
}
|
|
for c in constraints:
|
|
if isinstance(c, MarkerDistanceConstraint):
|
|
report["constraints"].append(
|
|
{
|
|
"kind": "distance",
|
|
"link_name": c.link_name,
|
|
"marker_id_a": c.marker_id_a,
|
|
"marker_id_b": c.marker_id_b,
|
|
"target_distance_m": c.target_distance_m,
|
|
"weight": c.weight,
|
|
"source": c.source,
|
|
}
|
|
)
|
|
else:
|
|
report["constraints"].append(
|
|
{
|
|
"kind": "joint_axis",
|
|
"parent_link": c.parent_link,
|
|
"child_link": c.child_link,
|
|
"marker_id_parent": c.marker_id_parent,
|
|
"marker_id_child": c.marker_id_child,
|
|
"joint_axis": [float(x) for x in c.joint_axis],
|
|
"target_delta_along_axis_m": c.target_delta_along_axis_m,
|
|
"weight": c.weight,
|
|
"source": c.source,
|
|
}
|
|
)
|
|
report_file = os.path.join(out_dir, "constraint_report.json")
|
|
save_json(report_file, report)
|
|
print(f"[INFO] Constraint report saved to {report_file}")
|
|
|
|
if args.saveObservationWeightReport:
|
|
obs_report = {
|
|
"schema_version": "1.0",
|
|
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
|
"summary": {
|
|
"num_weighted_observations": len(obs_weights),
|
|
},
|
|
"observation_weights": [
|
|
{
|
|
"marker_id": int(mid),
|
|
"observation_index": int(obs_idx),
|
|
"weight": float(w),
|
|
}
|
|
for (mid, obs_idx), w in sorted(obs_weights.items())
|
|
],
|
|
}
|
|
obs_file = os.path.join(out_dir, "observation_weight_report.json")
|
|
save_json(obs_file, obs_report)
|
|
print(f"[INFO] Observation-weight report saved to {obs_file}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|