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

vibe coding: diesmal GPT

Browse Source
This commit is contained in:
chk
2026-05-28 22:36:42 +02:00
parent 18f0e61f49
commit 409dca204d
12 changed files with 6524 additions and 1011 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1348
pipeline/2_Multiview.py
View File

File diff suppressed because it is too large Load Diff

1079
pipeline/2_Multiview_Weighted.py Normal file
View File

File diff suppressed because it is too large Load Diff

787
pipeline/2_Multiview_confidence_fehlt.py Normal file
View File

@@ -0,0 +1,787 @@
#!/usr/bin/env python3
"""
============================================================
STEP 2b — Simultane Multiview-Optimierung für Roboterpose
============================================================
Ziel:
Aus mehreren ArUco-Detektionsdateien die gemeinsame
Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose
sowie Marker-Weltpositionen ausgeben.
Eingabe:
--robot ../robot.json
--detections render_1a_aruco_detection.json render_1b_aruco_detection.json ...
--outDir .
Ausgabe:
multiview_pose.json
Hinweis:
Dieses Skript verwendet die Markerpositionen aus robot.json
als kinematische Constraints und optimiert gleichzeitig:
- Roboterzustand (x,y,z,a,b,c,e)
- Kameraextrinsische Parameter pro Bild
"""
import argparse
import datetime
import json
import math
import os
import time
from pathlib import Path
from typing import Any, Dict, List, Tuple
import cv2
import numpy as np
from scipy.optimize import least_squares
STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"]
# ------------------------------------------------------------------
# Constraint definitions and validation
# ------------------------------------------------------------------
class ConstraintResult:
"""Result of validating/applying a single constraint"""
def __init__(self, name: str, enabled: bool, reason: str = ""):
self.name = name
self.enabled = enabled
self.reason = reason
self.residuals = []
def __str__(self) -> str:
status = "✓ ENABLED" if self.enabled else "✗ DISABLED"
return f"{self.name:40s} {status:12s} {self.reason}"
def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]:
"""
Validate which constraints can be applied based on robot geometry.
Returns a dict of constraint_name -> ConstraintResult
"""
results = {}
# --- Constraint 1: Rigid body distances within each link ---
rigid_body_result = ConstraintResult("RigidBodyDistances", False)
try:
rigid_body_count = 0
for link_name in ['Arm1', 'Ellbow', 'Arm2']:
link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name]
if len(link_markers) >= 2:
rigid_body_count += 1
if rigid_body_count >= 2:
rigid_body_result.enabled = True
rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each"
else:
rigid_body_result.reason = "Not enough rigid links with multiple markers"
except Exception as e:
rigid_body_result.reason = f"Error: {str(e)}"
results['RigidBodyDistances'] = rigid_body_result
# --- Constraint 2: Fixed X-distances between links (rotation around X-axis) ---
inter_link_x_result = ConstraintResult("InterLinkXDistances", False)
try:
links_with_markers = set(m['link_name'] for m in robot_markers.values())
x_rotated_links = []
for link_name in ['Arm1', 'Ellbow']:
if link_name in links_with_markers:
link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name]
if len(link_markers) >= 1:
x_rotated_links.append(link_name)
if len(x_rotated_links) >= 2:
inter_link_x_result.enabled = True
inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}"
else:
inter_link_x_result.reason = "Not enough X-rotation links"
except Exception as e:
inter_link_x_result.reason = f"Error: {str(e)}"
results['InterLinkXDistances'] = inter_link_x_result
# --- Sanity check (not a hard constraint): Arm2 sin(a) dependency ---
arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)")
try:
arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2']
if len(arm2_markers) >= 2:
z_values = set(m['position_m'][2] for m in arm2_markers)
if len(z_values) > 1:
arm2_sina_result.enabled = True
arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed"
else:
arm2_sina_result.enabled = False
arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)"
else:
arm2_sina_result.enabled = False
arm2_sina_result.reason = "Not enough Arm2 markers"
except Exception as e:
arm2_sina_result.reason = f"Error: {str(e)}"
results['Arm2SinADependency'] = arm2_sina_result
return results
# ------------------------------------------------------------------
# JSON helpers
# ------------------------------------------------------------------
def load_json(path: str) -> Dict[str, Any]:
with open(path, 'r', encoding='utf-8') as f:
return json.load(f)
def save_json(data: Dict[str, Any], path: Path) -> None:
with open(path, 'w', encoding='utf-8') as f:
json.dump(data, f, indent=2)
# ------------------------------------------------------------------
# robot.json helpers
# ------------------------------------------------------------------
def resolve_scalar(value: Any, default: float = 0.0) -> float:
if value is None:
return default
if isinstance(value, (int, float)):
return float(value)
try:
return float(str(value).strip())
except ValueError:
return default
def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]:
if value is None:
return tuple(0.0 for _ in range(default_len))
if isinstance(value, (int, float, str)):
return (resolve_scalar(value),) + tuple(0.0 for _ in range(default_len - 1))
if isinstance(value, (list, tuple)):
resolved = [resolve_scalar(v) for v in value]
if len(resolved) < default_len:
resolved.extend([0.0] * (default_len - len(resolved)))
return tuple(resolved[:default_len])
return tuple(0.0 for _ in range(default_len))
def parse_metric_scale(robot: Dict[str, Any]) -> float:
rendering_info = robot.get('renderingInfo', {}) or {}
metric = rendering_info.get('metric', 'mm')
return 0.001 if str(metric).strip().lower() == 'mm' else 1.0
def normalize_axis(axis: Any) -> np.ndarray:
vec = np.asarray(axis, dtype=np.float64)
if vec.shape != (3,):
vec = vec.reshape(-1)[:3]
norm = np.linalg.norm(vec)
return vec / max(norm, 1e-9)
def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray:
x_deg, y_deg, z_deg = resolve_vector(euler_deg, 3)
x = math.radians(x_deg)
y = math.radians(y_deg)
z = math.radians(z_deg)
cx = math.cos(x)
sx = math.sin(x)
cy = math.cos(y)
sy = math.sin(y)
cz = math.cos(z)
sz = math.sin(z)
Rx = np.array([
[1.0, 0.0, 0.0],
[0.0, cx, -sx],
[0.0, sx, cx]
], dtype=np.float64)
Ry = np.array([
[cy, 0.0, sy],
[0.0, 1.0, 0.0],
[-sy, 0.0, cy]
], dtype=np.float64)
Rz = np.array([
[cz, -sz, 0.0],
[sz, cz, 0.0],
[0.0, 0.0, 1.0]
], dtype=np.float64)
return Rz @ Ry @ Rx
def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray:
T = np.eye(4, dtype=np.float64)
pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64)
T[:3, 3] = pos
T[:3, :3] = euler_deg_to_matrix(rotation_deg)
return T
def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray:
axis_vec = normalize_axis(axis)
theta = math.radians(angle_deg)
kx, ky, kz = axis_vec
c = math.cos(theta)
s = math.sin(theta)
v = 1.0 - c
R = np.array([
[kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s],
[ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s],
[kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c]
], dtype=np.float64)
T = np.eye(4, dtype=np.float64)
T[:3, :3] = R
return T
# ------------------------------------------------------------------
# Kinematics and marker extraction
# ------------------------------------------------------------------
def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]:
markers = {}
links = robot.get('links', {}) or {}
marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {}
default_size_mm = float(marker_defaults.get('size', 25.0))
for link_name, link_info in links.items():
for marker in link_info.get('markers', []) or []:
marker_id = int(marker.get('id', -1))
if marker_id < 0:
continue
pos = resolve_vector(marker.get('position', [0, 0, 0]), 3)
size_mm = float(marker.get('size', default_size_mm))
markers[marker_id] = {
'marker_id': marker_id,
'link_name': link_name,
'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64),
'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)),
'spin_deg': float(marker.get('spin', 0.0)),
'size_m': size_mm * scale,
}
return markers
def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]:
n = normalize_axis(normal)
candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64)
if abs(np.dot(n, candidate)) > 0.99:
candidate = np.array((1.0, 0.0, 0.0), dtype=np.float64)
x_dir = np.cross(candidate, n)
x_dir /= max(np.linalg.norm(x_dir), 1e-9)
y_dir = np.cross(n, x_dir)
if abs(spin_deg) > 1e-6:
theta = math.radians(spin_deg)
cos_t = math.cos(theta)
sin_t = math.sin(theta)
x_rot = x_dir * cos_t + np.cross(n, x_dir) * sin_t + n * np.dot(n, x_dir) * (1.0 - cos_t)
y_rot = y_dir * cos_t + np.cross(n, y_dir) * sin_t + n * np.dot(n, y_dir) * (1.0 - cos_t)
return x_rot, y_rot
return x_dir, y_dir
def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray:
half = marker['size_m'] * 0.5
x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg'])
corners = np.stack([
-x_dir * half + y_dir * half,
x_dir * half + y_dir * half,
x_dir * half - y_dir * half,
-x_dir * half - y_dir * half
], axis=0)
return marker['position_m'].reshape(1, 3) + corners
def build_link_chain(robot: Dict[str, Any]) -> List[str]:
links = robot.get('links', {}) or {}
ordered: List[str] = []
remaining = set(links.keys())
while remaining:
progress = False
for name in list(remaining):
parent = links[name].get('parent')
if not parent or parent in ordered:
ordered.append(name)
remaining.remove(name)
progress = True
if not progress:
raise RuntimeError('Cycle detected in robot link tree or missing parent link')
return ordered
def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]:
links = robot.get('links', {}) or {}
ordered_links = build_link_chain(robot)
transforms: Dict[str, np.ndarray] = {}
for link_name in ordered_links:
link_info = links[link_name] or {}
parent_name = link_info.get('parent')
parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64)
mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale
mount = transform_from_translation_rotation(
mount_translation,
link_info.get('mountRotation', [0, 0, 0])
)
joint_info = link_info.get('jointToParent', {}) or {}
joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale
joint = transform_from_translation_rotation(
joint_origin,
joint_info.get('rotation', [0, 0, 0])
)
motion = np.eye(4, dtype=np.float64)
joint_type = str(joint_info.get('type', 'fixed')).strip().lower()
control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower()
axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3)
if joint_type == 'linear':
motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale
elif joint_type == 'revolute':
motion = axis_angle_matrix(axis, state.get(control_var, 0.0))
transforms[link_name] = parent_transform @ mount @ joint @ motion
return transforms
def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray:
link_transform = link_transforms[marker['link_name']]
local = np.ones(4, dtype=np.float64)
local[:3] = marker['position_m']
world = link_transform @ local
return world[:3]
# ------------------------------------------------------------------
# Camera / observation helpers
# ------------------------------------------------------------------
def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]:
cam = detection_json['camera']
K = np.asarray(cam['camera_matrix'], dtype=np.float64)
D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1)
return K, D
def collect_views_and_observations(
detection_files: List[str],
robot_markers: Dict[int, Dict[str, Any]]
) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]:
views: List[Dict[str, Any]] = []
observations: List[Dict[str, Any]] = []
for idx, det_path in enumerate(detection_files):
detection_json = load_json(det_path)
K, D = load_intrinsics(detection_json)
views.append({
'index': idx,
'source_file': os.path.abspath(det_path),
'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'),
'image_file': detection_json.get('image', {}).get('image_file'),
'K': K,
'D': D
})
for det in detection_json.get('detections', []) or []:
marker_id = int(det.get('marker_id', -1))
if marker_id < 0 or marker_id not in robot_markers:
continue
image_points = det.get('image_points_px')
if isinstance(image_points, list) and len(image_points) == 4:
image_points = np.asarray(image_points, dtype=np.float64)
else:
center = resolve_vector(det.get('center_px', [0, 0]), 2)
image_points = np.asarray([center], dtype=np.float64)
confidence = float(det.get('confidence', 1.0))
marker = robot_markers[marker_id]
observations.append({
'view_index': idx,
'marker_id': marker_id,
'marker_link_corners': marker_object_corners(marker),
'image_points_px': image_points,
'confidence': max(0.01, min(1.0, confidence))
})
if len(views) == 0:
raise RuntimeError('No valid detection views found')
if len(observations) == 0:
raise RuntimeError('No marker observations matched robot.json markers')
return views, observations
def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray:
link_transform = link_transforms[marker['link_name']]
local = marker_object_corners(marker)
homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1)
world = (link_transform @ homogeneous.T).T
return world[:, :3]
def initial_camera_guess(
view: Dict[str, Any],
observations: List[Dict[str, Any]],
robot_markers: Dict[int, Dict[str, Any]],
default_state: Dict[str, float],
scale: float,
robot: Dict[str, Any]
) -> Tuple[np.ndarray, np.ndarray]:
object_points = []
image_points = []
link_transforms = compute_link_transforms(robot, default_state, scale)
for obs in observations:
if obs['view_index'] != view['index']:
continue
marker = robot_markers[obs['marker_id']]
object_points.append(compute_marker_world_corners(marker, link_transforms))
image_points.append(obs['image_points_px'])
if len(object_points) == 0:
return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64)
object_points = np.vstack(object_points)
image_points = np.vstack(image_points)
if object_points.shape[0] < 4:
return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64)
success, rvec, tvec = cv2.solvePnP(
object_points,
image_points,
view['K'],
view['D'],
flags=cv2.SOLVEPNP_ITERATIVE
)
if not success:
return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64)
return rvec, tvec
def project_points(
points_3d: np.ndarray,
rvec: np.ndarray,
tvec: np.ndarray,
K: np.ndarray,
D: np.ndarray
) -> np.ndarray:
projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D)
return projected.reshape(-1, 2)
# ------------------------------------------------------------------
# Optimization
# ------------------------------------------------------------------
def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray:
state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64)
cams = []
for rvec, tvec in camera_params:
cams.append(rvec.reshape(3))
cams.append(tvec.reshape(3))
return np.concatenate([state_vec] + cams)
def unpack_parameters(params: np.ndarray, n_views: int) -> Tuple[Dict[str, float], List[Tuple[np.ndarray, np.ndarray]]]:
robot_state = {STATE_KEYS[i]: float(params[i]) for i in range(len(STATE_KEYS))}
camera_params = []
offset = len(STATE_KEYS)
for _ in range(n_views):
rvec = params[offset:offset + 3].reshape(3, 1)
tvec = params[offset + 3:offset + 6].reshape(3, 1)
camera_params.append((rvec, tvec))
offset += 6
return robot_state, camera_params
def residuals_for_parameters(
params: np.ndarray,
views: List[Dict[str, Any]],
observations: List[Dict[str, Any]],
robot_markers: Dict[int, Dict[str, Any]],
robot: Dict[str, Any],
scale: float,
default_state: Dict[str, float]
) -> np.ndarray:
robot_state, camera_params = unpack_parameters(params, len(views))
link_transforms = compute_link_transforms(robot, robot_state, scale)
residuals = []
for obs in observations:
marker = robot_markers[obs['marker_id']]
world_corners = compute_marker_world_corners(marker, link_transforms)
rvec, tvec = camera_params[obs['view_index']]
proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D'])
diffs = proj - obs['image_points_px']
weight = math.sqrt(obs['confidence'])
residuals.extend((diffs * weight).reshape(-1))
for key in STATE_KEYS:
diff = robot_state[key] - default_state.get(key, 0.0)
if key in ('x', 'y', 'z', 'e'):
w = 0.001
else:
w = 0.01
residuals.append(diff * w)
return np.asarray(residuals, dtype=np.float64)
def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray:
if result.jac is None:
return np.full(n_params, float('nan'), dtype=np.float64)
J = result.jac
m, n = J.shape
JTJ = J.T @ J
try:
cov = np.linalg.pinv(JTJ)
except np.linalg.LinAlgError:
cov = np.linalg.pinv(JTJ + np.eye(n) * 1e-9)
residuals = result.fun
dof = max(1, m - n)
sigma2 = float(np.sum(residuals ** 2) / dof)
cov *= sigma2
return np.sqrt(np.diag(cov))
# ------------------------------------------------------------------
# Output assembly
# ------------------------------------------------------------------
def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
R, _ = cv2.Rodrigues(rvec)
return (-R.T @ tvec).reshape(3)
def build_output(
robot_state: Dict[str, float],
state_uncertainty: np.ndarray,
views: List[Dict[str, Any]],
camera_params: List[Tuple[np.ndarray, np.ndarray]],
observations: List[Dict[str, Any]],
robot_markers: Dict[int, Dict[str, Any]],
scale: float,
robot: Dict[str, Any],
robot_json_path: str
) -> Dict[str, Any]:
link_transforms = compute_link_transforms(robot, robot_state, scale)
marker_summary: Dict[int, Dict[str, Any]] = {}
for marker_id, marker in robot_markers.items():
marker_summary[marker_id] = {
'marker_id': marker_id,
'link_name': marker['link_name'],
'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(),
'size_m': marker['size_m'],
'observation_count': 0,
'mean_confidence': None,
'mean_reprojection_error_px': None,
'observations': []
}
per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary}
per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary}
link_transforms = compute_link_transforms(robot, robot_state, scale)
for obs in observations:
marker_id = obs['marker_id']
marker = robot_markers[marker_id]
object_points_m = compute_marker_world_corners(marker, link_transforms)
rvec, tvec = camera_params[obs['view_index']]
proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D'])
diffs = proj - obs['image_points_px']
errors = np.linalg.norm(diffs, axis=1)
repro_error = float(np.mean(errors))
per_marker_errors[marker_id].extend(errors.tolist())
per_marker_confidences[marker_id].append(obs['confidence'])
marker_summary[marker_id]['observation_count'] += 1
marker_summary[marker_id]['observations'].append({
'view_index': obs['view_index'],
'source_file': views[obs['view_index']]['source_file'],
'image_file': views[obs['view_index']]['image_file'],
'confidence': obs['confidence'],
'mean_reprojection_error_px': repro_error,
'corner_reprojection_errors_px': errors.tolist()
})
for marker_id, summary in marker_summary.items():
if summary['observation_count'] > 0:
summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id]))
summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id]))
camera_outputs = []
for idx, view in enumerate(views):
rvec, tvec = camera_params[idx]
cam_pos = camera_position_world(rvec, tvec)
observed_count = sum(1 for obs in observations if obs['view_index'] == idx)
camera_outputs.append({
'view_index': idx,
'source_file': view['source_file'],
'camera_id': view['camera_id'],
'camera_position_world_m': cam_pos.tolist(),
'rvec': rvec.reshape(-1).tolist(),
'tvec': tvec.reshape(-1).tolist(),
'intrinsics': {
'camera_matrix': view['K'].tolist(),
'distortion_coefficients': view['D'].reshape(-1).tolist()
},
'observation_count': observed_count
})
robot_pose_output = {
'state': {k: float(robot_state[k]) for k in STATE_KEYS},
'uncertainty': {
'x_mm': float(state_uncertainty[0]),
'y_mm': float(state_uncertainty[1]),
'z_mm': float(state_uncertainty[2]),
'a_deg': float(state_uncertainty[3]),
'b_deg': float(state_uncertainty[4]),
'c_deg': float(state_uncertainty[5]),
'e_mm': float(state_uncertainty[6])
},
'confidence': {
'x': float(math.exp(-state_uncertainty[0] / 10.0)),
'y': float(math.exp(-state_uncertainty[1] / 10.0)),
'z': float(math.exp(-state_uncertainty[2] / 10.0)),
'a': float(math.exp(-state_uncertainty[3] / 10.0)),
'b': float(math.exp(-state_uncertainty[4] / 10.0)),
'c': float(math.exp(-state_uncertainty[5] / 10.0)),
'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6])))
}
}
return {
'schema_version': '1.0',
'created_utc': datetime.datetime.utcnow().isoformat() + 'Z',
'source_robot_json': os.path.abspath(robot_json_path),
'source_detections': [view['source_file'] for view in views],
'robot_pose': robot_pose_output,
'camera_poses': camera_outputs,
'marker_positions': list(marker_summary.values())
}
# ------------------------------------------------------------------
# Main
# ------------------------------------------------------------------
def main() -> None:
parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics')
parser.add_argument('--robot', required=True, help='Path to robot.json')
parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files')
parser.add_argument('--outDir', required=True, help='Output directory')
parser.add_argument('--write-summary', action='store_true', help='Write summary file')
parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations')
args = parser.parse_args()
os.makedirs(args.outDir, exist_ok=True)
robot_json_path = os.path.abspath(args.robot)
robot = load_json(robot_json_path)
scale = parse_metric_scale(robot)
default_state = {
k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0)
for k in STATE_KEYS
}
robot_markers = extract_markers(robot, scale)
views, observations = collect_views_and_observations(args.detections, robot_markers)
camera_guesses = []
for view in views:
rvec, tvec = initial_camera_guess(view, observations, robot_markers, default_state, scale, robot)
camera_guesses.append((rvec, tvec))
x0 = pack_parameters(default_state, camera_guesses)
progress = {
'iter': 0,
'last_cost': None,
'last_print': time.time(),
'prev_x': x0.copy()
}
def progress_callback(xk: np.ndarray) -> None:
progress['iter'] += 1
now = time.time()
if progress['iter'] == 1 or now - progress['last_print'] >= 1.0:
res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state)
cost = 0.5 * float(np.dot(res, res))
delta_cost = None
convergence = ''
if progress['last_cost'] is not None:
delta_cost = cost - progress['last_cost']
if abs(delta_cost) < 1e-3:
convergence = ' stable'
elif delta_cost < 0:
convergence = ' improving'
else:
convergence = ' worsening'
step_norm = float(np.linalg.norm(xk - progress['prev_x']))
print(
f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}'
+ (f' delta={delta_cost:.4g}' if delta_cost is not None else '')
+ f' step={step_norm:.4g}'
+ convergence
)
progress['last_cost'] = cost
progress['last_print'] = now
progress['prev_x'] = xk.copy()
result = least_squares(
residuals_for_parameters,
x0,
args=(views, observations, robot_markers, robot, scale, default_state),
jac='2-point',
method='trf',
loss='soft_l1',
f_scale=1.0,
max_nfev=args.max_iter,
callback=progress_callback
)
robot_state, camera_params = unpack_parameters(result.x, len(views))
uncertainties = estimate_uncertainty(result, len(result.x))
output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path)
out_path = Path(args.outDir) / 'multiview_pose.json'
save_json(output, out_path)
print(f'Saved: {out_path}')
if args.write_summary:
summary_path = Path(args.outDir) / 'multiview_pose_summary.json'
summary = {
'final_cost': float(result.cost),
'status': int(result.status),
'message': result.message,
'robot_state': output['robot_pose'],
'camera_count': len(views),
'marker_count': len(robot_markers)
}
save_json(summary, summary_path)
print(f'Saved: {summary_path}')
if __name__ == '__main__':
main()

BIN
pipeline/__pycache__/2_Multiview.cpython-311.pyc
View File

Binary file not shown.

47
pipeline/camera_pose_summary.json
View File

@@ -1,47 +0,0 @@
{
"schema_version": "1.0",
"algorithm": "board_pnp_camera_pose",
"robot_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json",
"intrinsics_source": "embedded_in_detection_json",
"results": [
{
"detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json",
"output_file": "render_1a_aruco_detection_camera_pose.json",
"rmse_px": 53.69444650385422,
"median_px": 32.08915387366267,
"num_correspondences": 32,
"used_marker_ids": [
205,
206,
207,
208,
210,
211,
215,
217
]
},
{
"detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json",
"output_file": "render_1b_aruco_detection_camera_pose.json",
"rmse_px": 0.9028284747812563,
"median_px": 0.87428115526563,
"num_correspondences": 8,
"used_marker_ids": [
210,
215
]
},
{
"detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json",
"output_file": "render_1c_aruco_detection_camera_pose.json",
"rmse_px": 1.013497154241286,
"median_px": 0.98147711476875,
"num_correspondences": 8,
"used_marker_ids": [
210,
214
]
}
]
}

1796
pipeline/multiview_pose.json
View File

File diff suppressed because it is too large Load Diff

36
pipeline/multiview_pose_summary.json
View File

@@ -1,36 +0,0 @@
{
"final_cost": 24986.21700952237,
"status": 0,
"message": "The maximum number of function evaluations is exceeded.",
"robot_state": {
"state": {
"x": 100.65052085564044,
"y": 29.661249073864987,
"z": -35.948789524024555,
"a": -119.85307495241203,
"b": 21.99999999999959,
"c": 90.99999999999991,
"e": 10.000000000000025
},
"uncertainty": {
"x_mm": 1490.1455410993876,
"y_mm": 429.51650769405427,
"z_mm": 477.27281993275335,
"a_deg": 3533.703361743133,
"b_deg": 13152.589314453906,
"c_deg": 13152.589314528996,
"e_mm": 131525.9153573403
},
"confidence": {
"x": 1.922212631331247e-65,
"y": 2.219908541429891e-19,
"z": 1.8719954726562647e-21,
"a": 3.4136023686230135e-154,
"b": 0.0,
"c": 0.0,
"e": 0.36787944117144233
}
},
"camera_count": 3,
"marker_count": 23
}

36
pipeline/render_1a_aruco_detection.json
View File

@@ -1,6 +1,6 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-28T15:43:43Z",
"created_utc": "2026-05-28T20:31:58Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "713817fc-5f5b-486d-850a-2b1915ab7eac",
"observation_id": "11b87d79-31e1-4293-abe2-4327e2021077",
"type": "aruco",
"marker_id": 102,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.9494437061622057
},
{
"observation_id": "cc667670-1b39-4632-8f1c-1a5fdf0e0c7e",
"observation_id": "3f6119be-7ed5-4ba2-b0b8-f2eb85c0f4ac",
"type": "aruco",
"marker_id": 243,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.8899728634608616
},
{
"observation_id": "0cd34a97-e5d7-4bbf-aac9-a4e203b31583",
"observation_id": "a963cd6a-dea8-47d2-ba29-7410d1add84e",
"type": "aruco",
"marker_id": 210,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.5673043275049208
},
{
"observation_id": "5632f4b1-0db1-4a98-b8d3-9ebbbb4b126f",
"observation_id": "5da8c3b3-2b28-47b4-9132-c5225c5ba0bb",
"type": "aruco",
"marker_id": 247,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.8796777163094818
},
{
"observation_id": "75ae283a-9ba7-43bc-bb54-ba44e24e235f",
"observation_id": "b765e797-5dd9-4123-ac41-85b5606132a0",
"type": "aruco",
"marker_id": 246,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.8015179238063419
},
{
"observation_id": "7c280cd2-7b94-476a-be3e-7d89e6e35ed3",
"observation_id": "aca8152b-525f-45e7-9849-c46d2123a21c",
"type": "aruco",
"marker_id": 101,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.3325004465415643
},
{
"observation_id": "2e554970-1749-4924-ac98-3ac4debec51b",
"observation_id": "f0a37d05-7187-4ae1-a53a-2b0260b751b4",
"type": "aruco",
"marker_id": 215,
"marker_size_m": 0.025,
@@ -424,7 +424,7 @@
"confidence": 0.7952557920267838
},
{
"observation_id": "cc766bc2-4f75-4520-baf3-e10c62f6c25d",
"observation_id": "38cfa10c-246e-439d-ba19-ab664d030ccc",
"type": "aruco",
"marker_id": 124,
"marker_size_m": 0.025,
@@ -478,7 +478,7 @@
"confidence": 0.34091855704160245
},
{
"observation_id": "c7cc54fc-982b-46e8-b176-82eefba18404",
"observation_id": "95edc7e5-0883-4ab5-8b6a-1f35d392d98a",
"type": "aruco",
"marker_id": 229,
"marker_size_m": 0.025,
@@ -532,7 +532,7 @@
"confidence": 0.26810902547334975
},
{
"observation_id": "7cd2ae3d-740e-4005-a8b8-b23b0ea0082a",
"observation_id": "602d2bac-2427-4211-86be-9a2b5905a4e4",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -586,7 +586,7 @@
"confidence": 0.18665602922528673
},
{
"observation_id": "95fcb10e-a4e9-4d43-8950-8d0523fe95aa",
"observation_id": "308e1529-24a2-49e0-a7ad-6ac4fdead845",
"type": "aruco",
"marker_id": 198,
"marker_size_m": 0.025,
@@ -640,7 +640,7 @@
"confidence": 0.20150745250271476
},
{
"observation_id": "e14fdb7d-e877-4a87-9e58-430ac0f56f18",
"observation_id": "0159ed42-fec0-4b1c-a383-9ca83f5a320f",
"type": "aruco",
"marker_id": 211,
"marker_size_m": 0.025,
@@ -694,7 +694,7 @@
"confidence": 0.6412317666518965
},
{
"observation_id": "7a8a865f-1251-4024-9255-278cb4cb527f",
"observation_id": "4fa2ceb4-7915-4dba-b0f8-9700378eec0b",
"type": "aruco",
"marker_id": 208,
"marker_size_m": 0.025,
@@ -748,7 +748,7 @@
"confidence": 0.6280424706698967
},
{
"observation_id": "d920e29d-6dd3-41f5-affb-0d25e5bdb1f7",
"observation_id": "d3bc0898-720e-4180-b70a-57a9adc4a30e",
"type": "aruco",
"marker_id": 217,
"marker_size_m": 0.025,
@@ -802,7 +802,7 @@
"confidence": 0.35596573288593486
},
{
"observation_id": "4024cff0-440d-4688-add4-aaef71fd8566",
"observation_id": "d0d5506c-abf1-495f-bce9-33d4b374e599",
"type": "aruco",
"marker_id": 206,
"marker_size_m": 0.025,
@@ -856,7 +856,7 @@
"confidence": 0.33820423087105295
},
{
"observation_id": "7019c1d8-7fcf-44ef-8ea5-407577eff29c",
"observation_id": "7b5443c9-fb76-4270-8c88-32c5a20953c1",
"type": "aruco",
"marker_id": 205,
"marker_size_m": 0.025,
@@ -910,7 +910,7 @@
"confidence": 0.28724459014346065
},
{
"observation_id": "17746037-a944-46b0-a817-efcd1d0fddb5",
"observation_id": "ac8f8ef1-039a-4762-a4f6-8bdea9e3a4e4",
"type": "aruco",
"marker_id": 207,
"marker_size_m": 0.025,

22
pipeline/render_1b_aruco_detection.json
View File

@@ -1,6 +1,6 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-28T15:43:43Z",
"created_utc": "2026-05-28T20:31:58Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "b47ae404-2374-4364-bddc-c3e6282f9754",
"observation_id": "e2619c8d-750d-4334-9b42-cb95426ed979",
"type": "aruco",
"marker_id": 102,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.8913851020933449
},
{
"observation_id": "5643f91d-1fc0-43db-b40b-78ef116132a8",
"observation_id": "d1d011f0-a579-448e-ace9-ea148ecbc5e7",
"type": "aruco",
"marker_id": 124,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.4804167810936165
},
{
"observation_id": "eb068542-bc32-4241-9f94-ae93cba29146",
"observation_id": "1b5435e6-7883-434c-a9c7-63f4b8d750d1",
"type": "aruco",
"marker_id": 243,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.9132502955127223
},
{
"observation_id": "98da437d-8a22-4a73-8075-137b743bca43",
"observation_id": "a673d83e-73ff-4d2d-a412-470b6da12d56",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.9456847621099602
},
{
"observation_id": "f0ec764f-dbdf-4170-8cff-c3f5c940e82b",
"observation_id": "a755c678-9ad1-4af2-a744-a3c1a8ae0596",
"type": "aruco",
"marker_id": 247,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.7448005120854795
},
{
"observation_id": "bddd6015-4815-41b2-bd83-dd9acce3a41b",
"observation_id": "38397299-767f-490d-a47f-a3f09635e995",
"type": "aruco",
"marker_id": 246,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.7581700566520715
},
{
"observation_id": "6bb1f556-4a03-4bf1-90ce-59aedab7b2ee",
"observation_id": "273cc5e9-e6e4-4359-8fb1-02a33a935f0d",
"type": "aruco",
"marker_id": 215,
"marker_size_m": 0.025,
@@ -424,7 +424,7 @@
"confidence": 0.826449608683203
},
{
"observation_id": "dd82dba6-3cea-4e60-b64a-6ad94127dd37",
"observation_id": "8da984fe-3152-43d6-8a39-b4e9266c2184",
"type": "aruco",
"marker_id": 210,
"marker_size_m": 0.025,
@@ -478,7 +478,7 @@
"confidence": 0.7813266383036261
},
{
"observation_id": "ed953a44-f516-4275-bc07-b6857fe8f8ee",
"observation_id": "92d1be16-65a4-4855-a63c-4d21357d0a9d",
"type": "aruco",
"marker_id": 229,
"marker_size_m": 0.025,
@@ -532,7 +532,7 @@
"confidence": 0.18684041285479083
},
{
"observation_id": "27bccaa2-ec68-4f8b-991b-585a270ca491",
"observation_id": "5cbcbaf2-7581-4f25-81e3-85b4ce5a03b2",
"type": "aruco",
"marker_id": 198,
"marker_size_m": 0.025,

16
pipeline/render_1c_aruco_detection.json
View File

@@ -1,6 +1,6 @@
{
"schema_version": "1.0",
"created_utc": "2026-05-28T15:43:44Z",
"created_utc": "2026-05-28T20:31:59Z",
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
@@ -46,7 +46,7 @@
},
"detections": [
{
"observation_id": "981a123d-7560-4fbe-93f1-d47980472b76",
"observation_id": "6df4525f-58c7-4757-8a5e-130486fc123b",
"type": "aruco",
"marker_id": 102,
"marker_size_m": 0.025,
@@ -100,7 +100,7 @@
"confidence": 0.7012635429952238
},
{
"observation_id": "7676c338-2b2b-4bdb-864d-b6839291742b",
"observation_id": "c4dd28db-6041-4464-867a-96b25d671918",
"type": "aruco",
"marker_id": 122,
"marker_size_m": 0.025,
@@ -154,7 +154,7 @@
"confidence": 0.5749326438029929
},
{
"observation_id": "1476f720-1d72-4d94-b24b-b02e5e2d1a76",
"observation_id": "5bdf65ad-9a27-4a28-be1e-6e925df61c72",
"type": "aruco",
"marker_id": 243,
"marker_size_m": 0.025,
@@ -208,7 +208,7 @@
"confidence": 0.9491420540234864
},
{
"observation_id": "f1bcb96e-0cf8-4c69-a4c1-fbeb03650fa2",
"observation_id": "37b9dec9-2578-4765-a40a-516fbe0424f5",
"type": "aruco",
"marker_id": 246,
"marker_size_m": 0.025,
@@ -262,7 +262,7 @@
"confidence": 0.4780463267147134
},
{
"observation_id": "a8bf2494-ddd8-4bf6-b87d-42182112e4e3",
"observation_id": "a49c96d9-7884-4ddb-8c07-8f86130b5220",
"type": "aruco",
"marker_id": 247,
"marker_size_m": 0.025,
@@ -316,7 +316,7 @@
"confidence": 0.5106218488640142
},
{
"observation_id": "642b867d-25d1-4407-9011-f7a62feed435",
"observation_id": "4da1fc9d-a7a9-4dd8-a39b-a667b8d5d165",
"type": "aruco",
"marker_id": 214,
"marker_size_m": 0.025,
@@ -370,7 +370,7 @@
"confidence": 0.6041252446922665
},
{
"observation_id": "623e960d-2058-4672-b06c-5316a5861158",
"observation_id": "a88abb43-d181-4584-b59b-28c65a2fff45",
"type": "aruco",
"marker_id": 210,
"marker_size_m": 0.025,

17
robot.json
View File

@@ -95,6 +95,23 @@
"c": null,
"e": null
},
"multiview_quality": {
"combine_mode": "mean",
"size_ref_px": 50.0,
"border_ref_px": 120.0,
"center_ref_norm": 1.0,
"sharpness_ref": 2500.0,
"homography_ref": 0.18,
"size_factor": 1.0,
"aspect_factor": 1.0,
"border_factor": 1.0,
"center_factor": 1.0,
"sharpness_factor": 1.0,
"homography_factor": 1.0,
"normal_visibility_factor": 1.0,
"spin_factor": 1.0,
"weight_floor": 0.01
},
"movements": {
"x": null,
"y": null,

2351
vibeCoding/29.5.txt Normal file
View File

File diff suppressed because it is too large Load Diff