ChK/appRobotRender
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GPT runde

Browse Source
This commit is contained in:
chk
2026-05-29 12:55:24 +02:00
parent 2aaab15f34
commit 1008c9f7fd
12 changed files with 1017 additions and 737 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
pipeline/2_Multiview.py → pipeline/2_Multiview_Trial/2_Multiview.py
View File

691
pipeline/2_estimate_camera_pose_from_aruco_json.py
View File

@@ -1,691 +0,0 @@
#!/usr/bin/env python3
"""
2_estimate_camera_pose_from_aruco_json.py
Berechnet die Kameraposition im Maschinen-/Board-Koordinatensystem
aus:
1. einer ArUco-Detections-JSON
2. robots.json mit bekannten Marker-Positionen
NEU:
- Marker-Orientierungen unterstützt
- Default: Board-Marker zeigen nach +Z
- Qualitätsbewertung erweitert
- Speichert ALLE erkannten Marker
- Speichert auch fehlgeschlagene Lösungen
- Bewertet Kamerageometrie
- Bewertet Markerabdeckung
- Bewertet Sichtwinkel
- Bewertet Markeranzahl
- Speichert vollständige Rohdaten
Benötigt:
pip install opencv-python numpy
"""
import argparse
import json
import math
from pathlib import Path
import cv2
import numpy as np
# ============================================================
# Hilfsfunktionen
# ============================================================
def normalize(v):
n = np.linalg.norm(v)
if n < 1e-9:
return v
return v / n
def rotation_matrix_from_axes(x_axis, y_axis, z_axis):
R = np.column_stack([
normalize(x_axis),
normalize(y_axis),
normalize(z_axis)
])
return R.astype(np.float32)
def rvec_tvec_to_camera_pose(rvec, tvec):
"""
OpenCV:
X_cam = R * X_world + t
Kamera im Weltkoordinatensystem:
C = -R^T * t
"""
R_cw, _ = cv2.Rodrigues(rvec)
R_wc = R_cw.T
cam_pos = -R_wc @ tvec
return R_wc, cam_pos.reshape(3)
def rotation_matrix_to_euler_zyx(R):
yaw = math.degrees(math.atan2(R[1, 0], R[0, 0]))
sp = math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)
pitch = math.degrees(math.atan2(-R[2, 0], sp))
roll = math.degrees(math.atan2(R[2, 1], R[2, 2]))
return roll, pitch, yaw
# ============================================================
# Marker-Orientierung
# ============================================================
def get_marker_rotation(marker):
# Explizite Rotation vorhanden?
if "rotation_matrix" in marker:
return np.array(
marker["rotation_matrix"],
dtype=np.float32
)
# Default:
# Marker zeigt nach +Z
#
# x = rechts
# y = oben
# z = aus Board heraus (+Z)
x_axis = np.array([1, 0, 0], dtype=np.float32)
y_axis = np.array([0, 1, 0], dtype=np.float32)
z_axis = np.array([0, 0, 1], dtype=np.float32)
return rotation_matrix_from_axes(
x_axis,
y_axis,
z_axis
)
# ============================================================
# Marker Lookup
# ============================================================
def build_marker_lookup(robot_data):
marker_lookup = {}
length_units = str(robot_data.get("units", {}).get("length", "")).strip().lower()
length_scale = 1.0
if length_units in ("mm", "millimeter", "millimeters"):
length_scale = 1.0 / 1000.0
elif length_units in ("cm", "centimeter", "centimeters"):
length_scale = 1.0 / 100.0
markers = []
# Neues Format: links -> Board -> markers
links = robot_data.get("links", {})
board = links.get("Board")
if board and "markers" in board:
markers = board["markers"]
# Fallback für altes Format
if not markers:
markers = robot_data.get("Marker", [])
for marker in markers:
marker_id = int(marker.get("id", -1))
if marker_id < 0:
continue
# Nur absolute Marker verwenden
if "position" not in marker:
continue
pos = marker["position"]
if pos is None:
continue
if len(pos) != 3:
continue
rotation = get_marker_rotation(marker)
marker_lookup[marker_id] = {
"position": np.array(
pos,
dtype=np.float32
) * np.float32(length_scale),
"rotation": rotation,
"on": marker.get("on", "unknown")
}
return marker_lookup
# ============================================================
# Marker-Eckpunkte
# ============================================================
def build_marker_object_points(
marker_center_world,
marker_rotation,
marker_size_m):
half = marker_size_m / 2.0
# OpenCV Corner Reihenfolge
local = np.array([
[-half, half, 0.0],
[ half, half, 0.0],
[ half, -half, 0.0],
[-half, -half, 0.0],
], dtype=np.float32)
rotated = (marker_rotation @ local.T).T
return rotated + marker_center_world.reshape(1, 3)
# ============================================================
# Qualitätsmetriken
# ============================================================
def compute_marker_spread(points_3d):
if len(points_3d) < 2:
return 0.0
mins = np.min(points_3d, axis=0)
maxs = np.max(points_3d, axis=0)
diag = np.linalg.norm(maxs - mins)
return float(diag)
def compute_viewing_angles(
camera_position,
marker_lookup,
used_markers):
results = []
for marker_id in used_markers:
marker = marker_lookup[marker_id]
pos = marker["position"]
R = marker["rotation"]
normal = R[:, 2]
to_camera = normalize(camera_position - pos)
dot = np.clip(
np.dot(normal, to_camera),
-1.0,
1.0
)
angle = math.degrees(math.acos(dot))
results.append(angle)
if len(results) == 0:
return {
"mean": None,
"max": None
}
return {
"mean": float(np.mean(results)),
"max": float(np.max(results))
}
def compute_pose_quality(
rms,
max_err,
num_markers,
marker_spread,
viewing_angle_mean):
score = 100.0
# Reprojection Error
score -= rms * 8.0
# Max Error
score -= max_err * 2.0
# Wenige Marker
if num_markers < 2:
score -= 50
elif num_markers < 4:
score -= 25
elif num_markers < 6:
score -= 10
# Schlechte räumliche Verteilung
if marker_spread < 0.10:
score -= 30
elif marker_spread < 0.25:
score -= 15
# Schlechter Blickwinkel
if viewing_angle_mean is not None:
if viewing_angle_mean > 70:
score -= 25
elif viewing_angle_mean > 50:
score -= 10
score = max(0.0, min(100.0, score))
return float(score)
# ============================================================
# Main
# ============================================================
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"--detections",
default=None
)
parser.add_argument(
"--robots",
required=True
)
parser.add_argument(
"--min-confidence",
type=float,
default=0.5
)
parser.add_argument(
"--max-reprojection-error",
type=float,
default=3.0
)
parser.add_argument(
"--outDir",
default=None
)
args = parser.parse_args()
# ============================================================
# JSON laden
# ============================================================
with open(args.detections, "r", encoding="utf-8") as f:
detection_data = json.load(f)
with open(args.robots, "r", encoding="utf-8") as f:
robot_data = json.load(f)
# ============================================================
# Kamera
# ============================================================
K = np.array(
detection_data["camera"]["camera_matrix"],
dtype=np.float32
)
D = np.array(
detection_data["camera"]["distortion_coefficients"],
dtype=np.float32
).reshape(-1, 1)
# ============================================================
# Marker laden
# ============================================================
known_markers = build_marker_lookup(robot_data)
# ============================================================
# Punktlisten
# ============================================================
object_points = []
image_points = []
used_markers = []
rejected_markers = []
detections = detection_data["detections"]
for det in detections:
marker_id = int(det["marker_id"])
confidence = float(
det.get("confidence", 1.0)
)
reason = None
if confidence < args.min_confidence:
reason = "low_confidence"
elif marker_id not in known_markers:
reason = "unknown_marker"
if reason is not None:
rejected_markers.append({
"marker_id": marker_id,
"reason": reason,
"confidence": confidence
})
continue
marker_info = known_markers[marker_id]
marker_center_world = marker_info["position"]
marker_rotation = marker_info["rotation"]
marker_size = float(
det["marker_size_m"]
)
obj_pts = build_marker_object_points(
marker_center_world,
marker_rotation,
marker_size
)
img_pts = np.array(
det["image_points_px"],
dtype=np.float32
)
object_points.append(obj_pts)
image_points.append(img_pts)
used_markers.append(marker_id)
# ============================================================
# Ausgabe vorbereiten
# ============================================================
out = {
"success": False,
"camera_pose": None,
"quality": {},
"used_markers": [],
"rejected_markers": rejected_markers,
"all_detected_markers": [
int(d["marker_id"])
for d in detections
]
}
# ============================================================
# Zu wenige Marker
# ============================================================
if len(object_points) == 0:
out["quality"] = {
"error": "no_known_markers",
"num_detected_markers": len(detections),
"num_used_markers": 0
}
if args.outDir:
out_dir = Path(args.outDir)
out_dir.mkdir(parents=True, exist_ok=True)
out_file = out_dir / (
Path(args.detections).stem + ".camera_pose.json"
)
else:
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print("Keine bekannten Marker gefunden.")
print(out_file)
return
# ============================================================
# solvePnP
# ============================================================
object_points = np.concatenate(
object_points,
axis=0
)
image_points = np.concatenate(
image_points,
axis=0
)
success, rvec, tvec = cv2.solvePnP(
object_points,
image_points,
K,
D,
flags=cv2.SOLVEPNP_ITERATIVE
)
if not success:
out["quality"] = {
"error": "solvepnp_failed",
"num_used_markers": len(set(used_markers))
}
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print("solvePnP fehlgeschlagen")
return
# ============================================================
# Kamera Pose
# ============================================================
R_wc, cam_pos = rvec_tvec_to_camera_pose(
rvec,
tvec
)
roll, pitch, yaw = rotation_matrix_to_euler_zyx(
R_wc
)
# ============================================================
# Reprojektion
# ============================================================
projected, _ = cv2.projectPoints(
object_points,
rvec,
tvec,
K,
D
)
projected = projected.reshape(-1, 2)
reproj_error = np.linalg.norm(
projected - image_points,
axis=1
)
rms = float(
np.sqrt(np.mean(reproj_error ** 2))
)
max_err = float(
np.max(reproj_error)
)
# ============================================================
# Qualität
# ============================================================
marker_positions = np.array([
known_markers[mid]["position"]
for mid in set(used_markers)
])
marker_spread = compute_marker_spread(
marker_positions
)
viewing = compute_viewing_angles(
cam_pos,
known_markers,
set(used_markers)
)
quality_score = compute_pose_quality(
rms,
max_err,
len(set(used_markers)),
marker_spread,
viewing["mean"]
)
# ============================================================
# Ausgabe
# ============================================================
print()
print("==================================================")
print("KAMERA-POSE")
print("==================================================")
print()
print("Position [m]")
print(f"X = {cam_pos[0]:.6f}")
print(f"Y = {cam_pos[1]:.6f}")
print(f"Z = {cam_pos[2]:.6f}")
print()
print("Orientation [deg]")
print(f"Roll = {roll:.3f}")
print(f"Pitch = {pitch:.3f}")
print(f"Yaw = {yaw:.3f}")
print()
print("Reprojection")
print(f"RMS = {rms:.3f}px")
print(f"MAX = {max_err:.3f}px")
print()
print("Quality")
print(f"Score = {quality_score:.1f}/100")
print(f"Marker Spread = {marker_spread:.3f}m")
if viewing["mean"] is not None:
print(
f"Mean Viewing Angle = "
f"{viewing['mean']:.1f}deg"
)
# ============================================================
# JSON speichern
# ============================================================
out["success"] = True
out["camera_pose"] = {
"position_m": {
"x": float(cam_pos[0]),
"y": float(cam_pos[1]),
"z": float(cam_pos[2]),
},
"orientation_deg": {
"roll": float(roll),
"pitch": float(pitch),
"yaw": float(yaw),
},
"rotation_matrix_world_from_camera": (
R_wc.tolist()
)
}
out["quality"] = {
"pose_quality_score": quality_score,
"reprojection_rms_px": rms,
"reprojection_max_px": max_err,
"num_detected_markers": len(detections),
"num_used_markers": len(set(used_markers)),
"marker_spread_m": marker_spread,
"mean_viewing_angle_deg": viewing["mean"],
"max_viewing_angle_deg": viewing["max"],
"pose_stable":
max_err <= args.max_reprojection_error
}
out["used_markers"] = sorted(
list(set(used_markers))
)
out_file = Path(args.detections).with_suffix(
".camera_pose.json"
)
with open(out_file, "w", encoding="utf-8") as f:
json.dump(out, f, indent=2)
print()
print("Gespeichert:")
print(out_file)
if __name__ == "__main__":
main()

674
pipeline/3_multiview_bundle_adjustment.py Normal file
View File

@@ -0,0 +1,674 @@
#!/usr/bin/env python3
"""
3_multiview_bundle_adjustment.py
Multi-view ArUco marker position optimization with geometric constraints.
Strategy:
1. Triangulate markers from multi-view observations (rough initial estimate)
2. Optimize marker positions with constraints:
- Same-link distance constraints: markers on same link keep fixed relative distance
- Joint-axis constraints: markers on linked arms via joint keep distance along joint axis
3. Use bundle adjustment: minimize reprojection error + constraint violations
Key insight: We do NOT use robot.json marker positions (they're in different joint config).
We only use robot.json hierarchy to DEFINE constraints.
Dependencies: numpy, opencv-python, scipy
"""
from __future__ import annotations
import argparse
import json
import os
import sys
import time
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", {})
length_unit = str(units.get("length", "")).strip().lower()
if length_unit in ("mm", "millimeter", "millimeters"):
return 1.0 / 1000.0
elif length_unit in ("cm", "centimeter", "centimeters"):
return 1.0 / 100.0
return 1.0
# ===================================================================
# Constraint definitions (from robot.json structure ONLY)
# ===================================================================
class MarkerDistanceConstraint:
"""
Two markers on the same link must maintain a fixed distance.
We compute this distance from their relative positions in robot.json,
but scaled to meters. The ACTUAL 3D positions will be optimized,
but their distance must stay constant.
"""
def __init__(self, marker_id_a: int, marker_id_b: int,
link_name: str, target_distance: float, weight: float = 1.0):
self.marker_id_a = marker_id_a
self.marker_id_b = marker_id_b
self.link_name = link_name
self.target_distance = target_distance
self.weight = weight
class JointAxisConstraint:
"""
Two markers on linked arms (connected by joint around axis).
If joint axis is [1,0,0], then delta_x component must stay constant.
"""
def __init__(self, marker_id_parent: int, marker_id_child: int,
parent_link: str, child_link: str,
joint_axis: np.ndarray, # unit vector [x,y,z]
target_delta_along_axis: float,
weight: float = 1.0):
self.marker_id_parent = marker_id_parent
self.marker_id_child = marker_id_child
self.parent_link = parent_link
self.child_link = child_link
self.joint_axis = joint_axis / (np.linalg.norm(joint_axis) + 1e-9)
self.target_delta_along_axis = target_delta_along_axis
self.weight = weight
def extract_constraints_from_hierarchy(robot_data: Dict[str, Any],
length_scale: float) -> Tuple[Dict, List]:
"""
Extract constraint definitions from robot.json.
NOTE: We only extract CONSTRAINT TYPES, not actual marker positions.
Marker positions will come from triangulation.
Returns:
marker_to_link: {marker_id -> link_name}
constraints: List of constraint objects
"""
constraints = []
marker_to_link = {}
links = robot_data.get("links", {})
# Build map: link_name -> markers on that link
link_markers = {}
for link_name, link_data in links.items():
markers = link_data.get("markers", [])
marker_ids = []
marker_positions = {}
for marker in markers:
marker_id = int(marker.get("id", -1))
pos = marker.get("position", None)
if marker_id >= 0 and pos is not None and len(pos) == 3:
marker_ids.append(marker_id)
marker_positions[marker_id] = np.array(pos, dtype=np.float32) * np.float32(length_scale)
marker_to_link[marker_id] = link_name
link_markers[link_name] = (marker_ids, marker_positions)
# Extract same-link distance constraints
for link_name, (marker_ids, positions) in link_markers.items():
if len(marker_ids) >= 2:
# For each pair, compute their distance
for i in range(len(marker_ids)):
for j in range(i + 1, len(marker_ids)):
mid_a = marker_ids[i]
mid_b = marker_ids[j]
pos_a = positions[mid_a]
pos_b = positions[mid_b]
target_dist = float(np.linalg.norm(pos_b - pos_a))
constraint = MarkerDistanceConstraint(
mid_a, mid_b, link_name, target_dist, weight=1.0
)
constraints.append(constraint)
# Extract joint-axis constraints (if we have markers on parent and child links)
for link_name, link_data in links.items():
parent_link = link_data.get("parent", None)
if parent_link is None:
continue
joint_info = link_data.get("jointToParent", {})
if not joint_info:
continue
joint_axis = joint_info.get("axis", [1, 0, 0])
joint_axis = np.array(joint_axis, dtype=np.float32)
# Get markers on this link and parent link
child_marker_ids, child_positions = link_markers.get(link_name, ([], {}))
parent_marker_ids, parent_positions = link_markers.get(parent_link, ([], {}))
if len(child_marker_ids) > 0 and len(parent_marker_ids) > 0:
# Create constraint between first marker of parent and first marker of child
for mid_parent in parent_marker_ids:
for mid_child in child_marker_ids:
if mid_parent == mid_child:
continue
pos_parent = parent_positions[mid_parent]
pos_child = child_positions[mid_child]
delta = pos_child - pos_parent
target_delta_along_axis = float(
np.dot(delta, joint_axis)
)
constraint = JointAxisConstraint(
mid_parent,
mid_child,
parent_link,
link_name,
joint_axis,
target_delta_along_axis,
weight=0.5
)
constraints.append(constraint)
return marker_to_link, constraints
# ===
# Helper Function
#
def print_constraints_with_errors(
title: str,
constraints: List,
positions: Dict[int, np.ndarray]
) -> None:
print(f"\n[INFO] {title}")
for idx, constraint in enumerate(constraints):
# ----------------------------------------------------------
# Distance constraint
# ----------------------------------------------------------
if isinstance(constraint, MarkerDistanceConstraint):
if (
constraint.marker_id_a not in positions or
constraint.marker_id_b not in positions
):
continue
pos_a = positions[constraint.marker_id_a]
pos_b = positions[constraint.marker_id_b]
actual_dist = np.linalg.norm(pos_b - pos_a)
error_m = actual_dist - constraint.target_distance
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*1000:.2f} mm | "
f"actual={actual_dist*1000:.2f} mm | "
f"error={error_m*1000:+.2f} mm"
)
# ----------------------------------------------------------
# Joint-axis constraint
# ----------------------------------------------------------
elif isinstance(constraint, JointAxisConstraint):
if (
constraint.marker_id_parent not in positions or
constraint.marker_id_child not in positions
):
continue
pos_parent = positions[constraint.marker_id_parent]
pos_child = positions[constraint.marker_id_child]
delta = pos_child - pos_parent
actual = np.dot(delta, constraint.joint_axis)
error_m = actual - constraint.target_delta_along_axis
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*1000:.2f} mm | "
f"actual={actual*1000:.2f} mm | "
f"error={error_m*1000:+.2f} mm"
)
# ===================================================================
# Multi-view loading
# ===================================================================
def load_observations_and_poses(
detection_files: List[str],
pose_files: List[str]
) -> Tuple[Dict, List, List]:
"""Load observations and camera poses."""
if len(detection_files) != len(pose_files):
raise ValueError(f"Mismatch: {len(detection_files)} detections vs {len(pose_files)} poses")
marker_observations = {}
cameras = []
obs_metadata = []
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)
# Camera intrinsics
cam_section = det.get("camera", {})
K = np.array(cam_section.get("camera_matrix", []), dtype=np.float32).reshape(3, 3)
D = np.array(cam_section.get("distortion_coefficients", []), dtype=np.float32).reshape(-1, 1)
# Camera pose
pose_section = pose_data.get("camera_pose", {})
world_to_cam = pose_section.get("world_to_camera", {})
R_wc = np.array(world_to_cam.get("rotation_matrix", []), dtype=np.float32).reshape(3, 3)
t_wc = np.array(world_to_cam.get("translation_m", []), dtype=np.float32).reshape(3)
cameras.append((K, D, R_wc, t_wc))
# Extract observations
detections = det.get("detections", [])
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.float32)
# Undistort to normalized coordinates
pts = center_px.reshape(1, 1, 2).astype(np.float32)
und = cv2.undistortPoints(pts, K, D, P=None)
norm_coords = und.reshape(2).astype(np.float32)
if marker_id not in marker_observations:
marker_observations[marker_id] = []
marker_observations[marker_id].append((cam_idx, norm_coords))
return marker_observations, cameras, obs_metadata
# ===================================================================
# Initial triangulation
# ===================================================================
def triangulate_marker_initial(
marker_id: int,
observations: List[Tuple[int, np.ndarray]],
cameras: List[Tuple]
) -> Optional[np.ndarray]:
"""Rough triangulation using first two views."""
if len(observations) < 2:
return None
K1, D1, R_wc_1, t_wc_1 = cameras[observations[0][0]]
K2, D2, R_wc_2, t_wc_2 = cameras[observations[1][0]]
norm_coords_1 = observations[0][1]
norm_coords_2 = observations[1][1]
# Convert to pixel coordinates
x1_px = K1[0, 0] * norm_coords_1[0] + K1[0, 2]
y1_px = K1[1, 1] * norm_coords_1[1] + K1[1, 2]
x2_px = K2[0, 0] * norm_coords_2[0] + K2[0, 2]
y2_px = K2[1, 1] * norm_coords_2[1] + K2[1, 2]
P1 = K1 @ np.hstack([R_wc_1, t_wc_1.reshape(3, 1)])
P2 = K2 @ np.hstack([R_wc_2, t_wc_2.reshape(3, 1)])
try:
X_h = cv2.triangulatePoints(P1, P2,
np.array([[x1_px], [y1_px]]).astype(np.float32),
np.array([[x2_px], [y2_px]]).astype(np.float32))
X = (X_h[:3] / X_h[3]).reshape(3).astype(np.float32)
return X
except Exception:
return None
def initial_triangulation(
marker_observations: Dict[int, List[Tuple[int, np.ndarray]]],
cameras: List[Tuple]
) -> Dict[int, np.ndarray]:
"""Compute initial marker positions via triangulation."""
triangulated = {}
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
# ===================================================================
# Bundle adjustment with constraints
# ===================================================================
def bundle_adjustment_residuals(
marker_positions_flat: np.ndarray,
marker_ids: List[int],
marker_observations: Dict[int, List[Tuple[int, np.ndarray]]],
cameras: List[Tuple],
constraints: List,
lambda_constraint: float = 100.0
) -> np.ndarray:
"""
Compute residuals for bundle adjustment.
marker_positions_flat: [x0, y0, z0, x1, y1, z1, ...]
Returns: [reprojection_residuals, constraint_residuals]
"""
marker_dict = {}
for i, marker_id in enumerate(marker_ids):
marker_dict[marker_id] = marker_positions_flat[i*3:(i+1)*3]
residuals = []
# Reprojection residuals
for marker_id, observations in marker_observations.items():
if marker_id not in marker_dict:
continue
X_world = marker_dict[marker_id]
for cam_idx, norm_coords_obs in observations:
K, D, R_wc, t_wc = cameras[cam_idx]
# Project
X_cam = R_wc @ X_world + t_wc
if X_cam[2] > 0.001: # in front of camera
proj_norm = X_cam[:2] / X_cam[2]
residual = proj_norm - norm_coords_obs
residuals.append(residual[0])
residuals.append(residual[1])
# Constraint residuals
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 = np.linalg.norm(pos_b - pos_a)
residual = (actual_dist - constraint.target_distance) * constraint.weight * lambda_constraint
residuals.append(residual)
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_along_axis = np.dot(delta, constraint.joint_axis)
residual = (actual_delta_along_axis - constraint.target_delta_along_axis) * constraint.weight * lambda_constraint
residuals.append(residual)
return np.array(residuals, dtype=np.float64)
def optimize_with_constraints(
initial_positions: Dict[int, np.ndarray],
marker_observations: Dict[int, List[Tuple[int, np.ndarray]]],
cameras: List[Tuple],
constraints: List,
lambda_constraint: float = 100.0,
max_iterations: int = 50
) -> Dict[int, np.ndarray]:
"""
Optimize marker positions using scipy least squares.
"""
try:
from scipy.optimize import least_squares
except ImportError:
print("[WARN] scipy not available, skipping optimization. Using initial triangulation.")
return initial_positions
marker_ids = sorted(initial_positions.keys())
x0 = np.concatenate([initial_positions[mid] for mid in marker_ids])
def residuals_fn(x):
return bundle_adjustment_residuals(x, marker_ids, marker_observations, cameras,
constraints, 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 * len(marker_ids),
verbose=1
)
optimized_flat = result.x
optimized = {}
for i, marker_id in enumerate(marker_ids):
optimized[marker_id] = optimized_flat[i*3:(i+1)*3]
print(f"[INFO] Optimization complete. Final cost: {np.sum(result.fun**2):.6f}")
return optimized
# ===================================================================
# Main
# ===================================================================
def main() -> None:
parser = argparse.ArgumentParser(
description="Multi-view bundle adjustment with 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"
)
args = parser.parse_args()
if len(args.detections) != len(args.poses):
print(f"[ERROR] Mismatch: {len(args.detections)} vs {len(args.poses)}")
sys.exit(1)
# Load robot
robot_data = load_json(args.robot)
length_scale = get_length_scale(robot_data)
print("[STEP 1] Extract constraint definitions from robot.json hierarchy...")
marker_to_link, constraints = extract_constraints_from_hierarchy(robot_data, length_scale)
print(f"[INFO] Extracted {len(constraints)} constraints")
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"Marker {constraint.marker_id_a} <-> {constraint.marker_id_b} | "
f"TargetDistance={constraint.target_distance:.6f} m | "
f"Weight={constraint.weight}"
)
elif isinstance(constraint, JointAxisConstraint):
axis_str = np.array2string(
constraint.joint_axis,
precision=3,
suppress_small=True
)
print(
f" [{idx:03d}] JOINT_AXIS | "
f"{constraint.parent_link}({constraint.marker_id_parent}) -> "
f"{constraint.child_link}({constraint.marker_id_child}) | "
f"Axis={axis_str} | "
f"TargetDelta={constraint.target_delta_along_axis:.6f} m | "
f"Weight={constraint.weight}"
)
print("\n[STEP 2] Load observations and camera poses...")
marker_observations, cameras, _ = load_observations_and_poses(args.detections, args.poses)
print(f"[INFO] {len(cameras)} cameras, {len(marker_observations)} observed markers")
print("\n[STEP 3] Initial triangulation...")
initial_pos = initial_triangulation(marker_observations, cameras)
print(f"[INFO] Triangulated {len(initial_pos)} markers")
# ------------------------------------------------------------
# Save initial triangulated positions (before optimization)
# ------------------------------------------------------------
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.0",
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
"summary": {
"stage": "initial_triangulation",
"num_cameras": len(cameras),
"num_markers": len(initial_pos)
},
"markers": initial_output_markers
}
out_dir = args.outDir or os.path.dirname(args.detections[0]) or "."
os.makedirs(resolve_path(out_dir), exist_ok=True)
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}")
print("\n[STEP 4] Bundle adjustment with constraints...")
optimized_pos = optimize_with_constraints(
initial_pos, marker_observations, cameras, constraints,
lambda_constraint=args.lambdaWeight
)
print_constraints_with_errors(
"Constraint list AFTER optimization",
constraints,
optimized_pos
)
# Output
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.0",
"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_dir = args.outDir or os.path.dirname(args.detections[0]) or "."
os.makedirs(resolve_path(out_dir), exist_ok=True)
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 __name__ == "__main__":
main()

123
pipeline/aruco_positions_initial.json Normal file
View File

@@ -0,0 +1,123 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T07:55:17Z",
"summary": {
"stage": "initial_triangulation",
"num_cameras": 3,
"num_markers": 8
},
"markers": [
{
"marker_id": 122,
"position_m": [
0.18520598113536835,
-0.21684008836746216,
0.13996362686157227
],
"position_mm": [
185.2059783935547,
-216.840087890625,
139.963623046875
],
"link": "Arm2"
},
{
"marker_id": 198,
"position_m": [
0.12119491398334503,
-0.046654392033815384,
0.13240119814872742
],
"position_mm": [
121.19491577148438,
-46.65439224243164,
132.4011993408203
],
"link": "Arm1"
},
{
"marker_id": 210,
"position_m": [
0.020141778513789177,
-0.01963355578482151,
7.823568921594415e-06
],
"position_mm": [
20.14177894592285,
-19.633556365966797,
0.007823568768799305
],
"link": "Board"
},
{
"marker_id": 211,
"position_m": [
0.24960945546627045,
-0.009326362982392311,
-0.0005378762143664062
],
"position_mm": [
249.6094512939453,
-9.326362609863281,
-0.5378761887550354
],
"link": "Board"
},
{
"marker_id": 214,
"position_m": [
0.3498840034008026,
-0.009714338928461075,
-2.6824214728549123e-05
],
"position_mm": [
349.8840026855469,
-9.714339256286621,
-0.026824215427041054
],
"link": "Board"
},
{
"marker_id": 215,
"position_m": [
0.2497633397579193,
-0.0896778479218483,
-0.00011857203207910061
],
"position_mm": [
249.76333618164062,
-89.67784881591797,
-0.11857203394174576
],
"link": "Board"
},
{
"marker_id": 229,
"position_m": [
0.12001997977495193,
-0.13270698487758636,
0.14000186324119568
],
"position_mm": [
120.01998138427734,
-132.7069854736328,
140.00186157226562
],
"link": "Arm1"
},
{
"marker_id": 243,
"position_m": [
0.11871032416820526,
-0.17057909071445465,
0.09964759647846222
],
"position_mm": [
118.7103271484375,
-170.57908630371094,
99.64759826660156
],
"link": "Arm1"
}
]
}

123
pipeline/aruco_positions_optimized.json Normal file
View File

@@ -0,0 +1,123 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T07:55:18Z",
"summary": {
"num_cameras": 3,
"num_markers": 8,
"num_constraints": 124
},
"markers": [
{
"marker_id": 122,
"position_m": [
0.17089422820752076,
-0.25275415714542,
0.1764152549525782
],
"position_mm": [
170.89422820752077,
-252.75415714541998,
176.4152549525782
],
"link": "Arm2"
},
{
"marker_id": 198,
"position_m": [
0.11820089367579532,
-0.047715698531904785,
0.13484286564203027
],
"position_mm": [
118.20089367579533,
-47.715698531904785,
134.84286564203026
],
"link": "Arm1"
},
{
"marker_id": 210,
"position_m": [
0.019901746801827637,
-0.019634665689316648,
9.617254460632054e-06
],
"position_mm": [
19.901746801827638,
-19.63466568931665,
0.009617254460632054
],
"link": "Board"
},
{
"marker_id": 211,
"position_m": [
0.2499096083030422,
-0.00981801346837422,
7.989879950350478e-05
],
"position_mm": [
249.9096083030422,
-9.81801346837422,
0.07989879950350479
],
"link": "Board"
},
{
"marker_id": 214,
"position_m": [
0.3499095633806716,
-0.009897723015074432,
0.00018350870717776096
],
"position_mm": [
349.90956338067156,
-9.897723015074432,
0.18350870717776097
],
"link": "Board"
},
{
"marker_id": 215,
"position_m": [
0.24984585113464844,
-0.08981799168050893,
8.39071085193236e-05
],
"position_mm": [
249.84585113464843,
-89.81799168050892,
0.08390710851932359
],
"link": "Board"
},
{
"marker_id": 229,
"position_m": [
0.11749065839484758,
-0.137499932562534,
0.14103531580163794
],
"position_mm": [
117.49065839484759,
-137.499932562534,
141.03531580163795
],
"link": "Arm1"
},
{
"marker_id": 243,
"position_m": [
0.11810035461635551,
-0.17482765346898027,
0.10853212349724542
],
"position_mm": [
118.10035461635552,
-174.82765346898026,
108.53212349724542
],
"link": "Arm1"
}
]
}

24
pipeline/render_3a_aruco_detection.json
View File

@@ -1,12 +1,12 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:07Z",
"created_utc": "2026-05-29T07:55:16Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
},
"camera": {
"camera_id": "cam1",
"camera_id": "cam3",
"intrinsics_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render.npz",
"camera_matrix": [
[
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "f95b7df5-d56a-4e50-baae-ed98245255b0",
"observation_id": "2ee8a896-adc4-44a7-8e21-00bf2d2bdb29",
"type": "aruco",
"marker_id": 124,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.6862086405991146
},
{
"observation_id": "d6a89cdb-43b2-4494-84c8-47d5de39baa8",
"observation_id": "122847ec-48d6-477c-a833-61a1cbb5620b",
"type": "aruco",
"marker_id": 243,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.953171926139578
},
{
"observation_id": "05209fc7-239c-43ca-8cf8-7566f456041d",
"observation_id": "4efd7b96-c897-44c5-9f63-6dfccdb0dcf0",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.5964639370258038
},
{
"observation_id": "52848f64-67df-4213-91b2-ffdfe9064ee2",
"observation_id": "c7a43ad5-e272-4673-af80-a17d11161ae4",
"type": "aruco",
"marker_id": 102,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.3578323322772018
},
{
"observation_id": "7bc13599-644e-4dbd-8ad2-ad5f80056d8c",
"observation_id": "7d8958be-4ff2-4929-a78a-0e3239574df7",
"type": "aruco",
"marker_id": 229,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.4396423002158715
},
{
"observation_id": "41826dad-da5d-4fb1-87e4-757a2c21171a",
"observation_id": "dea83763-ba79-4211-ace5-2fb4693ec614",
"type": "aruco",
"marker_id": 210,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.23718801109435655
},
{
"observation_id": "f354d0f3-34a9-428e-887d-a4364525a2b0",
"observation_id": "04e86e9d-49e6-4cc3-b104-8cc4bddae5e1",
"type": "aruco",
"marker_id": 198,
"marker_size_m": 0.025,
@@ -424,7 +424,7 @@
"confidence": 0.27797680859006363
},
{
"observation_id": "447b944c-8faf-41e5-b724-af8f221c64fc",
"observation_id": "d844a883-c800-4e9c-8f99-fdb379cf9d6a",
"type": "aruco",
"marker_id": 205,
"marker_size_m": 0.025,
@@ -478,7 +478,7 @@
"confidence": 0.19676029488014599
},
{
"observation_id": "51c410f3-45e5-4d70-9bd8-535c1306e4cf",
"observation_id": "e1194640-f6cb-4af1-8fe3-41d19817b79e",
"type": "aruco",
"marker_id": 206,
"marker_size_m": 0.025,
@@ -532,7 +532,7 @@
"confidence": 0.23511362818048806
},
{
"observation_id": "61058f2a-9c08-4073-a43e-7a907cf8fb75",
"observation_id": "1aa5ff5a-71ef-4362-aeb0-2f000b6c7f37",
"type": "aruco",
"marker_id": 207,
"marker_size_m": 0.025,

8
pipeline/render_3a_camera_pose.json
View File

@@ -1,12 +1,12 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:09Z",
"created_utc": "2026-05-29T07:55:17Z",
"source": {
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3a_aruco_detection.json",
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json"
},
"camera": {
"camera_id": "cam1",
"camera_id": "cam3",
"camera_matrix": [
[
1777.77783203125,
@@ -123,7 +123,7 @@
],
"orientation_deg": {
"roll": 122.43758392333984,
"pitch": -35.883320699421276,
"pitch": -35.88331985473633,
"yaw": -19.852933883666992
}
},
@@ -202,7 +202,7 @@
],
"orientation_std_deg": {
"roll": 0.1051783179305808,
"pitch": 0.14376921142986104,
"pitch": 0.13780027424955138,
"yaw": 0.02230052560885805
}
}

22
pipeline/render_3b_aruco_detection.json
View File

@@ -1,12 +1,12 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:08Z",
"created_utc": "2026-05-29T07:55:16Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
},
"camera": {
"camera_id": "cam1",
"camera_id": "cam2",
"intrinsics_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render.npz",
"camera_matrix": [
[
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "df664593-ada8-4c44-a00f-8936a037e7eb",
"observation_id": "37e350d1-d7f5-4006-9e7c-eabb597891bb",
"type": "aruco",
"marker_id": 243,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.8522257252911212
},
{
"observation_id": "a7f8731a-1479-4db3-af34-2b5542362b93",
"observation_id": "fb1bfc74-794b-4065-b35e-f49b5e413a94",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.2874514216623714
},
{
"observation_id": "3cf6936d-c17e-46be-960e-fec43a1a8a34",
"observation_id": "6766568d-a40c-4c97-87fd-77e5243838fb",
"type": "aruco",
"marker_id": 229,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.5547001883002887
},
{
"observation_id": "1eeec489-e960-4f11-bcc5-d84cfd85678d",
"observation_id": "958853a3-10c7-486a-8985-8fa8eab7fb7a",
"type": "aruco",
"marker_id": 208,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.6072727689079809
},
{
"observation_id": "7f948349-46ae-4ada-bf71-a87a11853b58",
"observation_id": "0ba8ce36-cbf9-42b2-8829-3c1b5adcf247",
"type": "aruco",
"marker_id": 215,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.6168610191628993
},
{
"observation_id": "22b325e5-012e-458f-8841-176fddce4f01",
"observation_id": "3e48e695-6ede-4e3d-932c-161c06a03ac7",
"type": "aruco",
"marker_id": 214,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.5524643209674667
},
{
"observation_id": "297f15ef-5a04-4fde-ad31-715064472cab",
"observation_id": "386a4b8c-7efc-4cad-a7e0-c02ce8d15271",
"type": "aruco",
"marker_id": 211,
"marker_size_m": 0.025,
@@ -424,7 +424,7 @@
"confidence": 0.5730650050844824
},
{
"observation_id": "5a57e9e6-1e2e-4061-aafe-72b4d412b042",
"observation_id": "cb63df83-5a11-4455-89ae-c741e2c649dd",
"type": "aruco",
"marker_id": 198,
"marker_size_m": 0.025,
@@ -478,7 +478,7 @@
"confidence": 0.4163346774695628
},
{
"observation_id": "8b92d6ef-360b-40af-9d92-c1f0e013c0ee",
"observation_id": "36b8474a-1769-40d3-86ad-98031727ad2e",
"type": "aruco",
"marker_id": 210,
"marker_size_m": 0.025,

8
pipeline/render_3b_camera_pose.json
View File

@@ -1,12 +1,12 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:10Z",
"created_utc": "2026-05-29T07:55:17Z",
"source": {
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3b_aruco_detection.json",
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json"
},
"camera": {
"camera_id": "cam1",
"camera_id": "cam2",
"camera_matrix": [
[
1777.77783203125,
@@ -118,7 +118,7 @@
],
"orientation_deg": {
"roll": 129.43496704101562,
"pitch": 7.254831367062062,
"pitch": 7.254830837249756,
"yaw": 6.209517478942871
}
},
@@ -197,7 +197,7 @@
],
"orientation_std_deg": {
"roll": 0.38938838441106427,
"pitch": 0.37119422591459295,
"pitch": 0.36987460773433695,
"yaw": 0.11587592903072558
}
}

24
pipeline/render_3c_aruco_detection.json
View File

@@ -1,6 +1,6 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:09Z",
"created_utc": "2026-05-29T07:55:15Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "c428f7ca-c6c6-4208-994c-69a56fa87a1d",
"observation_id": "d51ce37d-1f1f-40ad-a6a2-a6c8f49b43ff",
"type": "aruco",
"marker_id": 219,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.7735450417034769
},
{
"observation_id": "00e68272-4cb7-4b93-98f8-efcc188685da",
"observation_id": "1806bda1-4af5-4faa-b0a3-0091bd35eabc",
"type": "aruco",
"marker_id": 218,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.7950075578135153
},
{
"observation_id": "ad0954b8-da38-4d82-825c-a05e65b0c5db",
"observation_id": "82259a28-11c6-4355-967d-d2c525cba254",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.8202207056111124
},
{
"observation_id": "94886815-8819-4fd0-9dd9-0579e8bb356a",
"observation_id": "97b9df19-3ff6-4315-8c74-470848b59a8e",
"type": "aruco",
"marker_id": 214,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.1700115058329765
},
{
"observation_id": "850986ee-399f-423c-8ffc-38c8b18b764d",
"observation_id": "6e25efa5-9cfb-4415-8be0-1c253eb68b9c",
"type": "aruco",
"marker_id": 215,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.9372326698512834
},
{
"observation_id": "fca3abcc-7c19-415d-bf78-c47360086985",
"observation_id": "8411563f-277d-44a1-b331-70ea22f8f316",
"type": "aruco",
"marker_id": 244,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.7137086345973509
},
{
"observation_id": "2a004519-5a1e-4b51-ac4e-f2e15b9f8ddd",
"observation_id": "20756782-3ad5-4dc0-9e2b-eea48226a4e7",
"type": "aruco",
"marker_id": 229,
"marker_size_m": 0.025,
@@ -424,7 +424,7 @@
"confidence": 0.8888336147757544
},
{
"observation_id": "d3d0a119-6c30-492f-9f4c-6e3fb13d8985",
"observation_id": "022ae18d-f34f-420e-8117-ec22aa3f52e4",
"type": "aruco",
"marker_id": 211,
"marker_size_m": 0.025,
@@ -478,7 +478,7 @@
"confidence": 0.8902520865668376
},
{
"observation_id": "93aa8eca-a954-4c99-a946-ce5f70ccfaaf",
"observation_id": "22a306fc-a02a-46c3-b9e1-d240ddb40ee6",
"type": "aruco",
"marker_id": 246,
"marker_size_m": 0.025,
@@ -532,7 +532,7 @@
"confidence": 0.7343609168978227
},
{
"observation_id": "f5108f58-1608-4b8f-b8a0-a8b31179e150",
"observation_id": "76dba603-04ec-4e63-bc5e-8ea448020025",
"type": "aruco",
"marker_id": 247,
"marker_size_m": 0.025,
@@ -586,7 +586,7 @@
"confidence": 0.72560382306579
},
{
"observation_id": "2ca775d6-ed4d-4ad0-85d1-1110d61be0a0",
"observation_id": "173b2222-1e89-405e-8e94-0c43f4773db1",
"type": "aruco",
"marker_id": 198,
"marker_size_m": 0.025,

6
pipeline/render_3c_camera_pose.json
View File

@@ -1,6 +1,6 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-29T06:03:10Z",
"created_utc": "2026-05-29T07:55:16Z",
"source": {
"detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3c_aruco_detection.json",
"robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json"
@@ -131,7 +131,7 @@
],
"orientation_deg": {
"roll": 149.0894775390625,
"pitch": 27.466156494334694,
"pitch": 27.466156005859375,
"yaw": 42.493896484375
}
},
@@ -210,7 +210,7 @@
],
"orientation_std_deg": {
"roll": 4.593062572697299e-13,
"pitch": 4.604301919561157e-13,
"pitch": 4.725448419113441e-13,
"yaw": 1.6845365562674818e-13
}
}

51
pipeline/run_pipeline_multiview.bat Normal file
View File

@@ -0,0 +1,51 @@
@echo off
REM 3_pipeline_multiview.bat
REM Multi-camera ArUco detection and pose estimation pipeline
REM Example: 3 camera views
REM Usage: run_pipeline_multiview.bat
cd /d "%~dp0"
set ROBOT_JSON=..\robot.json
set OUT_DIR=.
REM Camera 1
set IMG_1=render_3c.png
set NPZ_1=render.npz
set CAM_ID_1=cam1
REM Camera 2 (example - you need actual second camera image)
set IMG_2=render_3b.png
set NPZ_2=render.npz
set CAM_ID_2=cam2
REM Camera 3 (example)
set IMG_3=render_3a.png
set NPZ_3=render.npz
set CAM_ID_3=cam3
echo [STEP 1] Detect ArUco markers from all cameras
python 1_detect_aruco_observations.py -i %IMG_1% -npz %NPZ_1% -robot %ROBOT_JSON% -cameraId %CAM_ID_1% -outDir %OUT_DIR%
python 1_detect_aruco_observations.py -i %IMG_2% -npz %NPZ_2% -robot %ROBOT_JSON% -cameraId %CAM_ID_2% -outDir %OUT_DIR%
python 1_detect_aruco_observations.py -i %IMG_3% -npz %NPZ_3% -robot %ROBOT_JSON% -cameraId %CAM_ID_3% -outDir %OUT_DIR%
echo.
echo [STEP 2] Estimate camera poses from detections
python 2_estimate_camera_from_observations.py -i render_3c_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR%
python 2_estimate_camera_from_observations.py -i render_3b_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR%
python 2_estimate_camera_from_observations.py -i render_3a_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR%
echo.
echo [STEP 3] Triangulate marker positions from multi-view observations
python 3_multiview_bundle_adjustment.py ^
-det render_3a_aruco_detection.json ^
-det render_3b_aruco_detection.json ^
-det render_3c_aruco_detection.json ^
-pose render_3a_camera_pose.json ^
-pose render_3b_camera_pose.json ^
-pose render_3c_camera_pose.json ^
-robot %ROBOT_JSON% -lambdaWeight 100.0
echo.
echo [DONE] Pipeline complete