SceneMosaic: Efficient and Diverse Simulation-Ready Scene Generation via Hybrid Agentic Layout Evolution
Python
from __future__ import annotations
import ast
import json
import math
import random
import re
from dataclasses import dataclass, field
from pathlib import Path
import numpy as np
from .types import SceneArtifacts
@dataclass
class LayoutMemory:
output_path: Path
anchor_id: str
layout_id: str | None = None
initial_pose: list[dict] = field(default_factory=list)
rounds: list[dict] = field(default_factory=list)
def record_initial_pose(self, pose: list[dict] | None) -> None:
self.initial_pose = pose or []
self.write()
def record_round(
self,
round_index: int,
simulated_pose: list[dict] | None = None,
collisions: list[dict] | None = None,
modification_notes: list[str] | None = None,
revised_pose: list[dict] | None = None,
pose_expressions: list | None = None,
) -> None:
entry = None
for candidate in self.rounds:
if candidate.get("round_index") == round_index:
entry = candidate
break
if entry is None:
entry = {
"round_index": round_index,
"simulated_pose": None,
"collisions": None,
"modification_notes": None,
"revised_pose": None,
"pose_expressions": None,
}
self.rounds.append(entry)
if simulated_pose is not None:
entry["simulated_pose"] = simulated_pose
if collisions is not None:
entry["collisions"] = collisions
if modification_notes is not None:
entry["modification_notes"] = modification_notes
if revised_pose is not None:
entry["revised_pose"] = revised_pose
if pose_expressions is not None:
entry["pose_expressions"] = pose_expressions
self.write()
def _format_number(self, value: float | int | None) -> str:
if value is None:
return ""
if isinstance(value, (int, float)):
return str(round(float(value), 3))
return str(value)
def _format_pose_list(self, pose_list: list[dict] | None, include_rotation: bool) -> list[str]:
if not pose_list:
return ["- (none)"]
lines = []
for pose in pose_list:
if not isinstance(pose, dict):
continue
object_id = pose.get("object_id")
center = pose.get("center_2d")
if not isinstance(center, list) or len(center) < 2:
continue
u_val, v_val = center[0], center[1]
line = f"- {object_id}: u={self._format_number(u_val)}, v={self._format_number(v_val)}"
if include_rotation:
rotation = pose.get("rotation_degrees")
if rotation is not None:
line += f", rotation_degrees={self._format_number(rotation)}"
lines.append(line)
return lines or ["- (none)"]
def _format_collisions(self, collisions: list[dict] | None) -> list[str]:
if not collisions:
return ["- (none)"]
lines = []
for item in collisions:
if not isinstance(item, dict):
continue
a_id = item.get("a")
b_id = item.get("b")
contacts = item.get("contacts")
line = f"- {a_id} vs {b_id} (contacts={self._format_number(contacts)})"
lines.append(line)
return lines or ["- (none)"]
def _format_notes(self, notes: list[str] | None) -> list[str]:
if not notes:
return ["- (none)"]
return [f"- {note}" for note in notes]
def _format_pose_expressions(self, expressions: list | None) -> list[str]:
if not expressions:
return ["- (none)"]
lines = []
for expression in expressions:
if isinstance(expression, str):
lines.append(f"- {expression}")
else:
lines.append(f"- {json.dumps(expression, ensure_ascii=False)}")
return lines or ["- (none)"]
def write(self) -> None:
lines: list[str] = ["# Layout Memory", ""]
lines.append(f"- anchor_id: {self.anchor_id}")
if self.layout_id is not None:
lines.append(f"- layout_id: {self.layout_id}")
lines.append("")
lines.append("## Initial 2D Pose")
lines.extend(self._format_pose_list(self.initial_pose, include_rotation=True))
sorted_rounds = sorted(self.rounds, key=lambda item: item.get("round_index", 0))
for entry in sorted_rounds:
round_index = entry.get("round_index")
lines.append("")
lines.append(f"## Round {round_index}")
if entry.get("pose_expressions") is not None:
lines.append("- Original Pose Expressions")
lines.extend(self._format_pose_expressions(entry.get("pose_expressions")))
if entry.get("revised_pose") is not None:
lines.append("- Revised 2D Pose")
lines.extend(self._format_pose_list(entry.get("revised_pose"), include_rotation=True))
if entry.get("simulated_pose") is not None:
lines.append("- 2D Pose after Simulation ")
lines.extend(self._format_pose_list(entry.get("simulated_pose"), include_rotation=True))
if entry.get("collisions") is not None:
lines.append("- Possible Deep Collisions")
lines.extend(self._format_collisions(entry.get("collisions")))
if entry.get("modification_notes") is not None:
lines.append("- Modification Notes for Next Round")
lines.extend(self._format_notes(entry.get("modification_notes")))
self.output_path.parent.mkdir(parents=True, exist_ok=True)
self.output_path.write_text("\n".join(lines).rstrip() + "\n", encoding="utf-8")
class SceneGenerator:
"""Generate new scenes based on learned relationships and original artifacts."""
def __init__(self, original_artifacts: SceneArtifacts):
self.original_artifacts = original_artifacts
def _score_layout_quality(
self,
collision_pairs: list[dict] | None,
out_of_bounds_summary: dict | None,
) -> int:
collision_count = len(collision_pairs or [])
out_of_bounds_count = 0
if isinstance(out_of_bounds_summary, dict):
entries = out_of_bounds_summary.get("out_of_bounds_objects", [])
if isinstance(entries, list):
out_of_bounds_count = len(entries)
return max(0, 100 - 20 * (collision_count + out_of_bounds_count))
def _has_layout_issues(
self,
collision_pairs: list[dict] | None,
out_of_bounds_summary: dict | None,
) -> bool:
if collision_pairs:
return True
if not isinstance(out_of_bounds_summary, dict):
return False
entries = out_of_bounds_summary.get("out_of_bounds_objects", [])
return isinstance(entries, list) and bool(entries)
def generate_similar_layout_context(
self,
mapping_path: Path | None = None,
objects: list[dict] | None = None,
anchor_id: str | None = None,
) -> dict:
"""Create the layout context for generate_layout_description prompt."""
objects_by_id = {}
if objects:
for obj in objects:
if not isinstance(obj, dict):
continue
object_id = str(obj.get("id", ""))
objects_by_id[object_id] = {
"category": obj.get("category", ""),
"description": obj.get("description", ""),
}
objects_context = {
"objects": [],
}
def _round_numbers(value):
if isinstance(value, float | int):
return round(value, 2)
if isinstance(value, list):
return [_round_numbers(item) for item in value]
if isinstance(value, dict):
return {k: _round_numbers(v) for k, v in value.items()}
return value
anchor_category = None
if mapping_path and mapping_path.exists():
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
mapping_objects = mapping_payload.get("objects", [])
if isinstance(mapping_objects, list):
for obj_info in mapping_objects:
if not isinstance(obj_info, dict):
continue
object_id = str(obj_info.get("object_id", ""))
meta = objects_by_id.get(object_id)
if meta is None and object_id.startswith("object_"):
alt_id = f"obj_{object_id.split('_', 1)[1]}"
meta = objects_by_id.get(alt_id)
category = (meta or {}).get("category", object_id)
description = (meta or {}).get("description", object_id)
if anchor_id and object_id == anchor_id:
anchor_category = category
description += " (This object is the anchor and should not change in the new layout)"
obj_info = {k: v for k, v in obj_info.items() if k != "signed_distance_to_plane"}
obj_info["category"] = category
obj_info["description"] = description
objects_context["objects"].append(_round_numbers(obj_info))
return {"objects_context": objects_context}, anchor_category
def create_layout_memory(
self,
output_path: Path,
anchor_id: str,
layout_id: str | None = None,
) -> LayoutMemory:
return LayoutMemory(output_path=output_path, anchor_id=anchor_id, layout_id=layout_id)
def extract_2d_pose_centers(self, pose_payload: dict) -> list[dict]:
return self._extract_2d_pose_entries(pose_payload, include_rotation=False)
def extract_2d_pose_with_rotation(self, pose_payload: dict) -> list[dict]:
return self._extract_2d_pose_entries(pose_payload, include_rotation=True)
def extract_2d_pose_from_mapping(self, mapping_path: Path | None) -> list[dict]:
if not mapping_path or not mapping_path.exists():
return []
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
objects = mapping_payload.get("objects", [])
if not isinstance(objects, list):
return []
results = []
for obj in objects:
if not isinstance(obj, dict):
continue
center = obj.get("center_2d")
if not isinstance(center, list) or len(center) < 2:
continue
object_id = obj.get("object_id")
entry = {
"object_id": object_id,
"center_2d": [center[0], center[1]],
}
rotation = obj.get("rotation_degrees")
if rotation is not None:
entry["rotation_degrees"] = rotation
results.append(entry)
return results
def _parse_bbox_2d(self, obj: dict) -> list[float] | None:
bbox = obj.get("bbox_2d")
if not isinstance(bbox, list) or len(bbox) < 4:
return None
try:
return [float(bbox[0]), float(bbox[1]), float(bbox[2]), float(bbox[3])]
except (TypeError, ValueError):
return None
def summarize_out_of_bounds_objects(
self,
mapping_path: Path | None,
anchor_id: str | None,
) -> dict:
if not mapping_path or not mapping_path.exists():
return {
"anchor_id": anchor_id,
"anchor_bbox_2d": None,
"out_of_bounds_objects": [],
}
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
objects = mapping_payload.get("objects", [])
if not isinstance(objects, list):
return {
"anchor_id": anchor_id,
"anchor_bbox_2d": None,
"out_of_bounds_objects": [],
}
anchor_bbox = None
for obj in objects:
if not isinstance(obj, dict):
continue
if str(obj.get("object_id")) != str(anchor_id):
continue
anchor_bbox = self._parse_bbox_2d(obj)
break
summary = {
"anchor_id": anchor_id,
"anchor_bbox_2d": anchor_bbox,
"out_of_bounds_objects": [],
}
if anchor_bbox is None:
return summary
anchor_min_u, anchor_min_v, anchor_max_u, anchor_max_v = anchor_bbox
for obj in objects:
if not isinstance(obj, dict):
continue
object_id = obj.get("object_id")
if object_id is None or str(object_id) == str(anchor_id):
continue
bbox = self._parse_bbox_2d(obj)
if bbox is None:
continue
min_u, min_v, max_u, max_v = bbox
u_buffer = (max_u - min_u) / 2.0 if anchor_id.startswith("object_") else 0.0
v_buffer = (max_v - min_v) / 2.0 if anchor_id.startswith("object_") else 0.0
exceeded_sides = []
if min_u < anchor_min_u - u_buffer - 1e-2:
exceeded_sides.append("left")
if min_v < anchor_min_v - v_buffer - 1e-2:
exceeded_sides.append("bottom")
if max_u > anchor_max_u + u_buffer + 1e-2:
exceeded_sides.append("right")
if max_v > anchor_max_v + v_buffer + 1e-2:
exceeded_sides.append("top")
if exceeded_sides:
summary["out_of_bounds_objects"].append(
{
"object_id": object_id,
"bbox_2d": [min_u, min_v, max_u, max_v],
"exceeded_sides": exceeded_sides,
}
)
return summary
def format_out_of_bounds_modification_notes(self, out_of_bounds_summary: dict | None) -> list[str]:
if not isinstance(out_of_bounds_summary, dict):
return []
entries = out_of_bounds_summary.get("out_of_bounds_objects", [])
if not isinstance(entries, list):
return []
notes: list[str] = []
for entry in entries:
if not isinstance(entry, dict):
continue
object_id = entry.get("object_id")
if not object_id:
continue
exceeded_sides = entry.get("exceeded_sides", [])
if isinstance(exceeded_sides, list) and exceeded_sides:
side_text = ", ".join(str(side) for side in exceeded_sides)
notes.append(
f"Move ({object_id}) inward so its bbox stays inside the anchor boundary (exceeded: {side_text})."
)
else:
notes.append(f"Move ({object_id}) inward so it stays inside the anchor boundary.")
return notes
def merge_modification_notes(
self,
base_notes: list[str] | None,
out_of_bounds_notes: list[str] | None,
) -> list[str]:
merged: list[str] = []
for note_list in (base_notes or [], out_of_bounds_notes or []):
for note in note_list:
text = str(note).strip()
if not text:
continue
if text not in merged:
merged.append(text)
return merged
def _extract_mapping_pose_by_id(
self,
mapping_path: Path | None,
anchor_id: str | None = None,
) -> dict[str, dict[str, float | None]]:
if not mapping_path or not mapping_path.exists():
return {}
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
objects = mapping_payload.get("objects", [])
if not isinstance(objects, list):
return {}
pose_by_id: dict[str, dict[str, float | None]] = {}
for obj in objects:
if not isinstance(obj, dict):
continue
object_id = obj.get("object_id")
if object_id is None:
continue
object_id = str(object_id)
if anchor_id is not None and object_id == str(anchor_id):
continue
center = obj.get("center_2d")
if not isinstance(center, list) or len(center) < 2:
continue
try:
u_val = float(center[0])
v_val = float(center[1])
except (TypeError, ValueError):
continue
rotation = obj.get("rotation_degrees")
rotation_value = None
if isinstance(rotation, (int, float)):
rotation_value = float(rotation)
pose_by_id[object_id] = {
"u": u_val,
"v": v_val,
"rotation_degrees": rotation_value,
}
return pose_by_id
def summarize_pose_change(
self,
previous_mapping_path: Path | None,
current_mapping_path: Path | None,
*,
anchor_id: str | None = None,
) -> dict:
prev_pose = self._extract_mapping_pose_by_id(previous_mapping_path, anchor_id=anchor_id)
curr_pose = self._extract_mapping_pose_by_id(current_mapping_path, anchor_id=anchor_id)
shared_ids = sorted(set(prev_pose.keys()) & set(curr_pose.keys()))
deltas: list[dict] = []
max_translation = 0.0
max_rotation_delta = 0.0
for object_id in shared_ids:
prev_entry = prev_pose[object_id]
curr_entry = curr_pose[object_id]
dx = float(curr_entry["u"]) - float(prev_entry["u"])
dy = float(curr_entry["v"]) - float(prev_entry["v"])
translation = math.sqrt(dx * dx + dy * dy)
max_translation = max(max_translation, translation)
prev_rot = prev_entry.get("rotation_degrees")
curr_rot = curr_entry.get("rotation_degrees")
rotation_delta = 0.0
if isinstance(prev_rot, (int, float)) and isinstance(curr_rot, (int, float)):
rotation_delta = abs(float(curr_rot) - float(prev_rot))
max_rotation_delta = max(max_rotation_delta, rotation_delta)
deltas.append(
{
"object_id": object_id,
"translation": translation,
"rotation_delta": rotation_delta,
}
)
return {
"object_count": len(shared_ids),
"max_translation": max_translation,
"max_rotation_delta": max_rotation_delta,
"object_deltas": deltas,
}
def should_early_stop_refinement(
self,
previous_mapping_path: Path | None,
current_mapping_path: Path | None,
*,
anchor_id: str | None,
collision_pairs: list[dict] | None,
out_of_bounds_summary: dict | None,
movement_threshold: float,
rotation_tolerance: float,
) -> dict:
change_summary = self.summarize_pose_change(
previous_mapping_path,
current_mapping_path,
anchor_id=anchor_id,
)
object_count = int(change_summary.get("object_count", 0))
max_translation = float(change_summary.get("max_translation", 0.0))
max_rotation_delta = float(change_summary.get("max_rotation_delta", 0.0))
has_collision = bool(collision_pairs)
out_of_bounds_objects = []
if isinstance(out_of_bounds_summary, dict):
raw = out_of_bounds_summary.get("out_of_bounds_objects", [])
if isinstance(raw, list):
out_of_bounds_objects = raw
has_out_of_bounds = bool(out_of_bounds_objects)
should_stop = (
object_count > 0
and max_rotation_delta <= float(rotation_tolerance)
and max_translation < float(movement_threshold)
and not has_collision
and not has_out_of_bounds
)
return {
"should_stop": should_stop,
"object_count": object_count,
"max_translation": max_translation,
"max_rotation_delta": max_rotation_delta,
"has_collision": has_collision,
"has_out_of_bounds": has_out_of_bounds,
}
def _extract_2d_pose_entries(self, pose_payload: dict, include_rotation: bool) -> list[dict]:
if not isinstance(pose_payload, dict):
return []
poses = pose_payload.get("poses", [])
if not isinstance(poses, list):
return []
results = []
for pose in poses:
if not isinstance(pose, dict):
continue
coords = pose.get("center_coordinates", {})
if not isinstance(coords, dict):
continue
u_val = coords.get("u")
v_val = coords.get("v")
if u_val is None or v_val is None:
continue
object_id = pose.get("object_id") or pose.get("id")
entry = {
"object_id": object_id,
"center_2d": [u_val, v_val],
}
if include_rotation:
rotation = pose.get("rotation_degrees")
if rotation is not None:
entry["rotation_degrees"] = rotation
results.append(entry)
return results
def _load_grid_mapping(self, mapping_path: Path) -> tuple[int | None, dict, dict]:
grid_size = None
grid_map = {}
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
raw_grid_size = mapping_payload.get("grid_size")
if isinstance(raw_grid_size, int):
grid_size = raw_grid_size
grid_points = mapping_payload.get("grid_points", [])
if isinstance(grid_points, list):
for point in grid_points:
if not isinstance(point, dict):
continue
i = point.get("grid_i")
j = point.get("grid_j")
p3d = point.get("point_3d_original")
if isinstance(i, int) and isinstance(j, int) and isinstance(p3d, list) and len(p3d) == 3:
grid_map[(i, j)] = p3d
return grid_size, grid_map, mapping_payload
def _nearest_grid_point(self, grid_map: dict, u: float, v: float) -> list[float] | None:
best = None
best_dist = None
for (i, j), point in grid_map.items():
dist = (i - u) ** 2 + (j - v) ** 2
if best_dist is None or dist < best_dist:
best_dist = dist
best = point
return best
def _interpolate_grid_point(
self,
grid_map: dict,
u: float,
v: float,
max_index: int,
) -> list[float] | None:
if max_index < 0:
return None
u_clamped = max(0.0, min(float(max_index), u))
v_clamped = max(0.0, min(float(max_index), v))
i0 = int(math.floor(u_clamped))
j0 = int(math.floor(v_clamped))
i1 = min(i0 + 1, max_index)
j1 = min(j0 + 1, max_index)
p00 = grid_map.get((i0, j0))
p10 = grid_map.get((i1, j0))
p01 = grid_map.get((i0, j1))
p11 = grid_map.get((i1, j1))
if i0 == i1 and j0 == j1:
if p00:
return p00
return self._nearest_grid_point(grid_map, u_clamped, v_clamped)
if j0 == j1 and p00 and p10:
t = u_clamped - i0
return [
(1 - t) * p00[0] + t * p10[0],
(1 - t) * p00[1] + t * p10[1],
(1 - t) * p00[2] + t * p10[2],
]
if i0 == i1 and p00 and p01:
s = v_clamped - j0
return [
(1 - s) * p00[0] + s * p01[0],
(1 - s) * p00[1] + s * p01[1],
(1 - s) * p00[2] + s * p01[2],
]
if not (p00 and p10 and p01 and p11):
return self._nearest_grid_point(grid_map, u_clamped, v_clamped)
t = u_clamped - i0
s = v_clamped - j0
return [
(1 - t) * (1 - s) * p00[0] + t * (1 - s) * p10[0] + (1 - t) * s * p01[0] + t * s * p11[0],
(1 - t) * (1 - s) * p00[1] + t * (1 - s) * p10[1] + (1 - t) * s * p01[1] + t * s * p11[1],
(1 - t) * (1 - s) * p00[2] + t * (1 - s) * p10[2] + (1 - t) * s * p01[2] + t * s * p11[2],
]
def _quaternion_from_axis_angle(self, axis: list, angle_degrees: float) -> list:
angle_rad = math.radians(angle_degrees)
half_angle = angle_rad / 2.0
sin_half = math.sin(half_angle)
cos_half = math.cos(half_angle)
w = cos_half
x = axis[0] * sin_half
y = axis[1] * sin_half
z = axis[2] * sin_half
return [w, x, y, z]
def _camera_axes(self, camera_direction: list) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
forward = np.asarray(camera_direction, dtype=np.float32).reshape(3)
norm = np.linalg.norm(forward)
if norm > 1e-6:
forward = forward / norm
up_hint = np.array([0.0, 1.0, 0.0], dtype=np.float32)
right = np.cross(up_hint, forward)
if np.linalg.norm(right) <= 1e-8:
up_hint = np.array([0.0, 0.0, 1.0], dtype=np.float32)
right = np.cross(up_hint, forward)
right = right / np.linalg.norm(right)
up = np.cross(forward, right)
up = up / np.linalg.norm(up)
return right, up, forward
def _quaternion_to_matrix(self, quat: list | np.ndarray) -> np.ndarray:
q = np.asarray(quat, dtype=np.float32).reshape(4)
norm = np.linalg.norm(q)
if norm > 1e-8:
q = q / norm
w, x, y, z = q
return np.array(
[
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
],
dtype=np.float32,
)
def _matrix_to_quaternion(self, matrix: np.ndarray) -> list:
m = np.asarray(matrix, dtype=np.float32)
trace = float(m[0, 0] + m[1, 1] + m[2, 2])
if trace > 0.0:
s = math.sqrt(trace + 1.0) * 2.0
w = 0.25 * s
x = (m[2, 1] - m[1, 2]) / s
y = (m[0, 2] - m[2, 0]) / s
z = (m[1, 0] - m[0, 1]) / s
elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
s = math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
w = (m[2, 1] - m[1, 2]) / s
x = 0.25 * s
y = (m[0, 1] + m[1, 0]) / s
z = (m[0, 2] + m[2, 0]) / s
elif m[1, 1] > m[2, 2]:
s = math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
w = (m[0, 2] - m[2, 0]) / s
x = (m[0, 1] + m[1, 0]) / s
y = 0.25 * s
z = (m[1, 2] + m[2, 1]) / s
else:
s = math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
w = (m[1, 0] - m[0, 1]) / s
x = (m[0, 2] + m[2, 0]) / s
y = (m[1, 2] + m[2, 1]) / s
z = 0.25 * s
return [float(w), float(x), float(y), float(z)]
def _euler_from_matrix_xyz(self, matrix: np.ndarray) -> tuple[float, float, float]:
r00, r10, r20 = matrix[0, 0], matrix[1, 0], matrix[2, 0]
r21, r22 = matrix[2, 1], matrix[2, 2]
if r20 < -0.995:
x = 0.0
y = math.pi / 2
z = math.atan2(matrix[0, 1], matrix[1, 1])
elif r20 > 0.995:
x = 0.0
y = -math.pi / 2
z = -math.atan2(matrix[0, 1], matrix[1, 1])
else:
x = math.atan2(r21, r22)
y = math.asin(-r20)
z = math.atan2(r10, r00)
return x, y, z
def _matrix_from_euler_xyz(self, x: float, y: float, z: float) -> np.ndarray:
cx, sx = math.cos(x), math.sin(x)
cy, sy = math.cos(y), math.sin(y)
cz, sz = math.cos(z), math.sin(z)
return np.array(
[
[cz * cy, cz * sy * sx - sz * cx, cz * sy * cx + sz * sx],
[sz * cy, sz * sy * sx + cz * cx, sz * sy * cx - cz * sx],
[-sy, cy * sx, cy * cx],
],
dtype=np.float32,
)
def update_rotation_about_camera_direction(
self,
rotation_degrees: float,
original_quaternion: list,
camera_direction: list,
) -> list:
"""Update the Euler angle around camera_direction and return a new quaternion."""
quat = np.asarray(original_quaternion, dtype=np.float32).reshape(-1)
if quat.size != 4:
raise ValueError("Original rotation must have 4 elements.")
right, up, forward = self._camera_axes(camera_direction)
basis = np.stack([right, up, forward], axis=1)
rotation_world = self._quaternion_to_matrix(quat)
rotation_local = basis.T @ rotation_world @ basis
euler_x, euler_y, _ = self._euler_from_matrix_xyz(rotation_local)
euler_z = math.radians(float(rotation_degrees))
rotation_local = self._matrix_from_euler_xyz(euler_x, euler_y, euler_z)
rotation_world = basis @ rotation_local @ basis.T
return self._matrix_to_quaternion(rotation_world)
def generate_posed_layouts(
self,
pose_output_path: Path,
pose_payload: dict,
render_object_ids: list,
camera_direction: list,
layout_dir: Path,
anchor_id: str,
render_transform_path: Path | None = None,
) -> None:
"""Generate layout JSON files from 2D pose estimates.
Args:
pose_output_path: Path to the 2D pose output JSON file
pose_payload: Parsed pose payload dict
render_object_ids: List of object IDs that were rendered
camera_direction: Camera direction vector for this anchor
layout_dir: Source layout directory (layout_params_simulated)
anchor_id: ID of the anchor object
"""
layout_output_dir = pose_output_path.parent / "layout_params"
layout_output_dir.mkdir(parents=True, exist_ok=True)
# Extract pose list from payload
poses = pose_payload.get("poses", [])
if not isinstance(poses, list):
return
# Build a mapping of pose object_id to pose data
pose_map = {}
for pose in poses:
if isinstance(pose, dict):
obj_id = pose.get("object_id")
if obj_id:
pose_map[str(obj_id)] = pose
# Normalize camera direction
camera_dir_array = np.array(camera_direction, dtype=np.float32)
camera_dir_norm = np.linalg.norm(camera_dir_array)
if camera_dir_norm > 1e-6:
camera_direction_normalized = camera_dir_array / camera_dir_norm
else:
camera_direction_normalized = camera_dir_array
# Align object rotation with the normalized scene coordinate system in the rendering script
rot_fix = 0.0
if abs(camera_direction_normalized[0]) < 1e-6 and abs(camera_direction_normalized[2]) < 1e-6:
if render_transform_path and render_transform_path.exists():
with render_transform_path.open("r", encoding="utf-8") as f:
render_transform = json.load(f)
transform_info = render_transform.get("transform_info", {})
rotation_matrix = transform_info.get("rotation_matrix_col_major")
if isinstance(rotation_matrix, list) and len(rotation_matrix) == 3:
rot_mat = np.asarray(rotation_matrix, dtype=np.float32).reshape(3, 3)
forward_vec = rot_mat @ np.array([0.0, 0.0, 1.0], dtype=np.float32)
rot_fix = math.degrees(math.atan2(float(forward_vec[0]), float(forward_vec[2])))
# Process each object in render_object_ids
for obj_id in render_object_ids:
obj_id_str = str(obj_id)
# Load source layout from layout_dir
source_layout_path = layout_dir / f"{obj_id_str}.json"
if not source_layout_path.exists():
print(f"Warning: Source layout not found for {obj_id_str}")
continue
with source_layout_path.open("r", encoding="utf-8") as f:
source_layout = json.load(f)
# If object not in pose_map, it means it wasn't moved (anchor case)
if obj_id_str not in pose_map or obj_id_str == anchor_id:
# Copy anchor layout as-is
output_layout_path = layout_output_dir / f"{obj_id_str}.json"
with output_layout_path.open("w", encoding="utf-8") as f:
json.dump(source_layout, f, ensure_ascii=False, indent=2)
continue
pose_data = pose_map[obj_id_str]
# Extract 3D coordinates from pose
coords_3d = pose_data.get("center_coordinates_3d", {})
if not isinstance(coords_3d, dict):
print(f"Warning: Invalid 3D coordinates for {obj_id_str}")
continue
x = coords_3d.get("x")
y = coords_3d.get("y")
z = coords_3d.get("z")
if x is None or y is None or z is None:
print(f"Warning: Incomplete 3D coordinates for {obj_id_str}")
continue
# Extract rotation in degrees
rotation_degrees = pose_data.get("rotation_degrees", None)
# Create new layout with source rotation/scale
original_rotation = source_layout.get("rotation", [[1, 0, 0, 0]])
new_layout = {
"translation": [[x, y, z]],
"rotation": original_rotation,
"scale": source_layout.get("scale", [[1, 1, 1]]),
}
# Apply rotation
if rotation_degrees is not None:
# Get original rotation quaternion
if isinstance(original_rotation, list) and len(original_rotation) > 0:
if len(original_rotation) == 4 and all(
isinstance(val, (int, float)) for val in original_rotation
):
orig_quat = original_rotation
else:
orig_quat = original_rotation[0]
else:
orig_quat = [1, 0, 0, 0]
try:
new_quat = self.update_rotation_about_camera_direction(
rotation_degrees + rot_fix,
orig_quat,
camera_direction_normalized.tolist(),
)
except ValueError:
print(f"Warning: Invalid original quaternion for {obj_id_str}, skipping rotation")
new_quat = orig_quat
new_layout["rotation"] = [new_quat]
else:
new_layout["rotation"] = original_rotation
# Save new layout
output_layout_path = layout_output_dir / f"{obj_id_str}.json"
with output_layout_path.open("w", encoding="utf-8") as f:
json.dump(new_layout, f, ensure_ascii=False, indent=2)
def convert_2d_pose_to_3d(self, mapping_path: Path | None, pose_payload: dict) -> dict:
if not mapping_path or not mapping_path.exists():
return pose_payload
if not isinstance(pose_payload, dict):
return pose_payload
poses = pose_payload.get("poses")
if not isinstance(poses, list) or not poses:
return pose_payload
grid_size, grid_map, mapping_payload = self._load_grid_mapping(mapping_path)
if not grid_map:
return pose_payload
if isinstance(grid_size, int) and grid_size > 1:
max_index = grid_size - 1
else:
max_i = max(i for i, _ in grid_map.keys())
max_j = max(j for _, j in grid_map.keys())
max_index = max(max_i, max_j)
signed_distance_by_id = {}
mapping_objects = mapping_payload.get("objects", [])
if isinstance(mapping_objects, list):
for obj in mapping_objects:
if not isinstance(obj, dict):
continue
object_id = obj.get("object_id")
signed_distance = obj.get("signed_distance_to_plane")
if object_id is not None and isinstance(signed_distance, (int, float)):
signed_distance_by_id[str(object_id)] = float(signed_distance)
plane_normal = None
plane_info = mapping_payload.get("plane", {})
if isinstance(plane_info, dict):
normal = plane_info.get("normal")
if isinstance(normal, list) and len(normal) == 3:
plane_normal = np.asarray(normal, dtype=np.float32)
norm = np.linalg.norm(plane_normal)
if norm > 1e-6:
plane_normal = plane_normal / norm
else:
plane_normal = None
for pose in poses:
if not isinstance(pose, dict):
continue
coords = pose.get("center_coordinates", {})
if not isinstance(coords, dict):
continue
u = coords.get("u")
v = coords.get("v")
try:
u_val = float(u)
v_val = float(v)
except (TypeError, ValueError):
continue
point_3d = self._interpolate_grid_point(grid_map, u_val, v_val, max_index)
if not point_3d:
continue
object_id = pose.get("object_id") or pose.get("id")
signed_distance = None
if object_id is not None:
signed_distance = signed_distance_by_id.get(str(object_id))
if signed_distance is not None and plane_normal is not None:
point_3d = [
point_3d[0] + plane_normal[0] * signed_distance,
point_3d[1] + plane_normal[1] * signed_distance,
point_3d[2] + plane_normal[2] * signed_distance,
]
pose["center_coordinates_3d"] = {
"x": point_3d[0],
"y": point_3d[1],
"z": point_3d[2],
}
return pose_payload
_POSE_REF_PATTERN = re.compile(r"[A-Za-z_][A-Za-z0-9_]*(?:\.[A-Za-z_][A-Za-z0-9_]*)+")
def _build_pose_context(self, mapping_payload: dict) -> dict:
context = {}
objects = mapping_payload.get("objects", [])
if not isinstance(objects, list):
return context
for obj in objects:
if not isinstance(obj, dict):
continue
object_id = obj.get("object_id")
if object_id is None:
continue
object_id = str(object_id)
entry = {}
center = obj.get("center_2d")
if isinstance(center, list) and len(center) >= 2:
entry["center"] = {
"u": float(center[0]),
"v": float(center[1]),
}
size = obj.get("size_2d")
if isinstance(size, list) and len(size) >= 2:
entry["size"] = {
"u": float(size[0]),
"v": float(size[1]),
}
bbox = obj.get("bbox_2d")
if isinstance(bbox, list) and len(bbox) >= 4:
entry["bbox"] = {
"min_u": float(bbox[0]),
"min_v": float(bbox[1]),
"max_u": float(bbox[2]),
"max_v": float(bbox[3]),
}
rotation_degrees = obj.get("rotation_degrees")
if isinstance(rotation_degrees, (int, float)):
entry["rotation_degrees"] = float(rotation_degrees)
if entry:
context[object_id] = entry
return context
def _normalize_pose_section(self, section: str) -> str:
section = section.strip().lower()
if section in {"center", "center_2d", "center2d"}:
return "center"
if section in {"size", "size_2d", "size2d"}:
return "size"
if section in {"bbox", "bbox_2d", "bbox2d"}:
return "bbox"
if section in {"rotation", "rotation_degrees"}:
return "rotation_degrees"
return section
def _lookup_pose_reference(self, reference: str, pose_context: dict) -> float | None:
parts = [part.strip() for part in reference.split(".") if part.strip()]
if len(parts) < 2:
return None
object_id = parts[0]
obj = pose_context.get(object_id)
if not isinstance(obj, dict):
return None
if len(parts) == 2:
key = self._normalize_pose_section(parts[1])
if key == "rotation_degrees":
value = obj.get("rotation_degrees")
return float(value) if isinstance(value, (int, float)) else None
return None
section = self._normalize_pose_section(parts[1])
key = parts[2]
if section == "center":
value = (obj.get("center") or {}).get(key)
return float(value) if isinstance(value, (int, float)) else None
if section == "size":
value = (obj.get("size") or {}).get(key)
return float(value) if isinstance(value, (int, float)) else None
if section == "bbox":
value = (obj.get("bbox") or {}).get(key)
return float(value) if isinstance(value, (int, float)) else None
if section == "rotation_degrees":
value = obj.get("rotation_degrees")
return float(value) if isinstance(value, (int, float)) else None
return None
def _parse_pose_target(self, target: str) -> tuple[str, str, str | None] | None:
if not target:
return None
parts = [part.strip() for part in target.split(".") if part.strip()]
if len(parts) < 2:
return None
object_id = parts[0]
if len(parts) == 2:
section = self._normalize_pose_section(parts[1])
if section == "rotation_degrees":
return object_id, "rotation_degrees", None
return None
section = self._normalize_pose_section(parts[1])
if section == "center":
axis = parts[2]
if axis in {"u", "v"}:
return object_id, "center", axis
if section == "rotation_degrees":
return object_id, "rotation_degrees", None
return None
def _parse_pose_assignment(self, statement: object) -> tuple[str, str] | None:
target = None
expression = None
if isinstance(statement, dict):
target = statement.get("target") or statement.get("lhs") or statement.get("left")
expression = statement.get("expression") or statement.get("rhs") or statement.get("value")
elif isinstance(statement, str):
raw = statement.strip().rstrip(";")
if "+=" in raw:
left, right = raw.split("+=", 1)
target = left.strip()
expression = f"{target} + ({right.strip()})"
elif "-=" in raw:
left, right = raw.split("-=", 1)
target = left.strip()
expression = f"{target} - ({right.strip()})"
elif "=" in raw:
left, right = raw.split("=", 1)
target = left.strip()
expression = right.strip()
if not target or not expression:
return None
return str(target), str(expression)
def _rewrite_pose_expression(self, expression: str, pose_context: dict) -> tuple[str | None, dict | None]:
variables: dict[str, float] = {}
missing_reference = False
def _replace(match: re.Match) -> str:
nonlocal missing_reference
reference = match.group(0)
value = self._lookup_pose_reference(reference, pose_context)
if value is None:
missing_reference = True
return "0"
key = f"ref_{len(variables)}"
variables[key] = float(value)
return key
rewritten = self._POSE_REF_PATTERN.sub(_replace, expression)
if missing_reference:
return None, None
return rewritten, variables
def _safe_eval_expression(self, expression: str, variables: dict[str, float]) -> float | None:
try:
tree = ast.parse(expression, mode="eval")
except SyntaxError:
return None
def _eval(node: ast.AST) -> float:
if isinstance(node, ast.Expression):
return _eval(node.body)
if isinstance(node, ast.Constant):
if isinstance(node.value, (int, float)):
return float(node.value)
raise ValueError("Unsupported constant")
if isinstance(node, ast.Name):
if node.id in variables:
return float(variables[node.id])
raise ValueError("Unknown variable")
if isinstance(node, ast.UnaryOp) and isinstance(node.op, (ast.UAdd, ast.USub)):
operand = _eval(node.operand)
return operand if isinstance(node.op, ast.UAdd) else -operand
if isinstance(node, ast.BinOp) and isinstance(
node.op, (ast.Add, ast.Sub, ast.Mult, ast.Div)
):
left = _eval(node.left)
right = _eval(node.right)
if isinstance(node.op, ast.Add):
return left + right
if isinstance(node.op, ast.Sub):
return left - right
if isinstance(node.op, ast.Mult):
return left * right
if isinstance(node.op, ast.Div):
return left / right
raise ValueError("Unsupported expression")
try:
return float(_eval(tree))
except Exception:
return None
def _update_center(self, obj_entry: dict, axis: str, value: float) -> None:
center = obj_entry.get("center") or {}
previous_u = center.get("u")
previous_v = center.get("v")
if axis == "u":
center["u"] = float(value)
elif axis == "v":
center["v"] = float(value)
obj_entry["center"] = center
if previous_u is None or previous_v is None:
return
new_u = center.get("u")
new_v = center.get("v")
if not isinstance(new_u, (int, float)) or not isinstance(new_v, (int, float)):
return
dx = float(new_u) - float(previous_u)
dy = float(new_v) - float(previous_v)
bbox = obj_entry.get("bbox")
if isinstance(bbox, dict):
for key, delta in (("min_u", dx), ("max_u", dx), ("min_v", dy), ("max_v", dy)):
if key in bbox and isinstance(bbox[key], (int, float)):
bbox[key] = float(bbox[key]) + delta
obj_entry["bbox"] = bbox
elif isinstance(obj_entry.get("size"), dict):
size = obj_entry["size"]
half_u = float(size.get("u", 0.0)) / 2.0
half_v = float(size.get("v", 0.0)) / 2.0
obj_entry["bbox"] = {
"min_u": float(new_u) - half_u,
"min_v": float(new_v) - half_v,
"max_u": float(new_u) + half_u,
"max_v": float(new_v) + half_v,
}
def resolve_pose_expressions(
self,
mapping_path: Path | None,
pose_payload: dict,
anchor_id: str | None = None,
) -> dict:
if not mapping_path or not mapping_path.exists():
return pose_payload
if not isinstance(pose_payload, dict):
return pose_payload
pose_expressions = pose_payload.get("pose_expressions")
if not pose_expressions:
if "poses" in pose_payload:
pose_payload["poses"] = []
return pose_payload
with mapping_path.open("r", encoding="utf-8") as f:
mapping_payload = json.load(f)
pose_context = self._build_pose_context(mapping_payload)
original_rotation_by_object = {
object_id: obj_entry.get("rotation_degrees")
for object_id, obj_entry in pose_context.items()
if isinstance(obj_entry, dict)
}
updated_objects: set[str] = set()
rotation_updates: set[str] = set()
if isinstance(pose_expressions, list):
for statement in pose_expressions:
parsed = self._parse_pose_assignment(statement)
if not parsed:
continue
target, expression = parsed
target_info = self._parse_pose_target(target)
if not target_info:
continue
object_id, field, axis = target_info
if anchor_id and object_id == anchor_id:
continue
if object_id not in pose_context:
continue
rewritten, variables = self._rewrite_pose_expression(expression, pose_context)
if rewritten is None or variables is None:
continue
value = self._safe_eval_expression(rewritten, variables)
if value is None:
continue
obj_entry = pose_context.get(object_id)
if not isinstance(obj_entry, dict):
continue
if field == "center" and axis in {"u", "v"}:
self._update_center(obj_entry, axis, float(value))
updated_objects.add(object_id)
elif field == "rotation_degrees":
obj_entry["rotation_degrees"] = float(value)
updated_objects.add(object_id)
rotation_updates.add(object_id)
if not updated_objects:
return pose_payload
resolved_poses = []
for object_id in sorted(updated_objects):
obj_entry = pose_context.get(object_id) or {}
center = obj_entry.get("center") or {}
if not isinstance(center.get("u"), (int, float)) or not isinstance(center.get("v"), (int, float)):
continue
entry = {
"object_id": object_id,
"center_coordinates": {
"u": float(center["u"]),
"v": float(center["v"]),
},
}
if object_id in rotation_updates:
rotation = obj_entry.get("rotation_degrees")
if isinstance(rotation, (int, float)):
entry["rotation_degrees"] = float(rotation)
elif isinstance(original_rotation_by_object.get(object_id), (int, float)):
entry["rotation_degrees"] = float(original_rotation_by_object[object_id])
resolved_poses.append(entry)
pose_payload["poses"] = resolved_poses
return pose_payload