106 lines
3.4 KiB
Python
106 lines
3.4 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
check_robot_marker_uniqueness.py
|
|
|
|
Standalone checker for duplicate / invalid marker IDs in robot.json.
|
|
|
|
Use this before running bundle adjustment:
|
|
python check_robot_marker_uniqueness.py -robot robot.json
|
|
|
|
Exit codes:
|
|
0 = no duplicates found
|
|
1 = duplicates or structural problems found
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import os
|
|
import sys
|
|
from collections import defaultdict
|
|
from typing import Any, Dict, List, Tuple
|
|
|
|
|
|
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 main() -> None:
|
|
parser = argparse.ArgumentParser(description="Check marker ID uniqueness in robot.json")
|
|
parser.add_argument("-robot", "--robot", default="../../data/robot/robot.json", help="Path to robot.json")
|
|
parser.add_argument(
|
|
"--strict",
|
|
action="store_true",
|
|
help="Return exit code 1 if any duplicate or invalid marker is found",
|
|
)
|
|
args = parser.parse_args()
|
|
|
|
robot = load_json(args.robot)
|
|
links = robot.get("links", {}) or {}
|
|
|
|
id_locations: Dict[int, List[Tuple[str, int, str]]] = defaultdict(list)
|
|
invalid_entries: List[str] = []
|
|
total_markers = 0
|
|
|
|
for link_name, link_data in links.items():
|
|
markers = link_data.get("markers", []) or []
|
|
for idx, marker in enumerate(markers):
|
|
total_markers += 1
|
|
marker_id = marker.get("id", None)
|
|
if marker_id is None:
|
|
invalid_entries.append(f"link='{link_name}' marker_index={idx}: missing id")
|
|
continue
|
|
try:
|
|
marker_id_int = int(marker_id)
|
|
except Exception:
|
|
invalid_entries.append(f"link='{link_name}' marker_index={idx}: non-integer id={marker_id!r}")
|
|
continue
|
|
|
|
name = str(marker.get("name", f"marker_{marker_id_int}"))
|
|
id_locations[marker_id_int].append((link_name, idx, name))
|
|
|
|
pos = marker.get("position", None)
|
|
if pos is None or not isinstance(pos, list) or len(pos) != 3:
|
|
invalid_entries.append(
|
|
f"link='{link_name}' marker_id={marker_id_int}: invalid/missing 3D position"
|
|
)
|
|
|
|
duplicates = {mid: locs for mid, locs in id_locations.items() if len(locs) > 1}
|
|
|
|
print(f"[INFO] Total marker entries: {total_markers}")
|
|
print(f"[INFO] Unique marker IDs : {len(id_locations)}")
|
|
print(f"[INFO] Duplicate IDs : {len(duplicates)}")
|
|
print(f"[INFO] Invalid entries : {len(invalid_entries)}")
|
|
|
|
if duplicates:
|
|
print("\n[DUPLICATES]")
|
|
for mid in sorted(duplicates.keys()):
|
|
print(f" marker_id={mid}")
|
|
for link_name, idx, name in duplicates[mid]:
|
|
print(f" - link='{link_name}' marker_index={idx} name='{name}'")
|
|
|
|
if invalid_entries:
|
|
print("\n[INVALID ENTRIES]")
|
|
for item in invalid_entries:
|
|
print(f" - {item}")
|
|
|
|
if duplicates or invalid_entries:
|
|
print("\n[WARN] robot.json is not clean for deterministic constraint generation.")
|
|
if args.strict:
|
|
sys.exit(1)
|
|
else:
|
|
print("\n[OK] No duplicate marker IDs found.")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|