Arbeiten mit GPT
This commit is contained in:
chk
@@ -16,6 +16,17 @@ import numpy as np
|
||||
# Utilities
|
||||
# ------------------------------------------------------------
|
||||
|
||||
|
||||
def resolve_path(path):
|
||||
path = os.path.expanduser(path)
|
||||
|
||||
# Absoluter Pfad → direkt verwenden
|
||||
if os.path.isabs(path):
|
||||
return path
|
||||
|
||||
# Relativer Pfad → absolut machen (auf Basis aktuellem cwd)
|
||||
return os.path.abspath(path)
|
||||
|
||||
def load_intrinsics_npz(npz_path: str):
|
||||
data = np.load(npz_path)
|
||||
|
||||
@@ -40,6 +51,9 @@ def load_intrinsics_npz(npz_path: str):
|
||||
|
||||
def load_robot_vision_config(robot_json_path: str):
|
||||
|
||||
|
||||
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, 'r', encoding='utf-8') as f:
|
||||
robot = json.load(f)
|
||||
|
||||
@@ -339,7 +353,9 @@ def main():
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
os.makedirs(args.outDir, exist_ok=True)
|
||||
out_dir = resolve_path(args.outDir)
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Load robot vision config
|
||||
@@ -354,7 +370,10 @@ def main():
|
||||
# Load image
|
||||
# --------------------------------------------------------
|
||||
|
||||
image = cv2.imread(args.image)
|
||||
|
||||
image_path = resolve_path(args.image)
|
||||
image = cv2.imread(image_path)
|
||||
|
||||
|
||||
if image is None:
|
||||
raise RuntimeError(f'Cannot read image: {args.image}')
|
||||
@@ -370,7 +389,9 @@ def main():
|
||||
# Intrinsics
|
||||
# --------------------------------------------------------
|
||||
|
||||
K, D = load_intrinsics_npz(args.intrinsics)
|
||||
|
||||
intrinsics_path = resolve_path(args.intrinsics)
|
||||
K, D = load_intrinsics_npz(intrinsics_path)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Detection
|
||||
@@ -562,7 +583,7 @@ def main():
|
||||
input_base = os.path.splitext(input_filename)[0]
|
||||
|
||||
out_json = os.path.join(
|
||||
args.outDir,
|
||||
out_dir,
|
||||
f'{input_base}_aruco_detection.json'
|
||||
)
|
||||
|
||||
|
||||
@@ -170,7 +170,7 @@ class ObservationQualityConfig:
|
||||
def _load_nested_quality_config(src: Dict[str, Any]) -> Dict[str, Any]:
|
||||
if not isinstance(src, dict):
|
||||
return {}
|
||||
for key in ("multiview_quality", "multiviewQuality", "quality_config", "multiview"):
|
||||
for key in ("multiview_quality","multiview_calculation", "multiviewQuality", "quality_config", "multiview"):
|
||||
v = src.get(key)
|
||||
if isinstance(v, dict):
|
||||
return v
|
||||
|
||||
2471
pipeline/2_Multiview_Trial/multiview_pose.json
Normal file
2471
pipeline/2_Multiview_Trial/multiview_pose.json
Normal file
File diff suppressed because it is too large
Load Diff
73
pipeline/2_Multiview_Trial/multiview_pose_summary.json
Normal file
73
pipeline/2_Multiview_Trial/multiview_pose_summary.json
Normal file
@@ -0,0 +1,73 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-28T21:58:12.453984Z",
|
||||
"source_robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json",
|
||||
"source_detections": [
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_2a_aruco_detection.json",
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_2b_aruco_detection.json",
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_2c_aruco_detection.json",
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_2d_aruco_detection.json"
|
||||
],
|
||||
"solver": {
|
||||
"final_cost": 22931.22178571188,
|
||||
"status": 0,
|
||||
"message": "The maximum number of function evaluations is exceeded."
|
||||
},
|
||||
"robot_pose": {
|
||||
"state": {
|
||||
"x": 6.55912456537666,
|
||||
"y": 4.460952073670867,
|
||||
"z": 30.7175200969605,
|
||||
"a": 11.566185446462232,
|
||||
"b": 1.999999999999993,
|
||||
"c": 8.999999999999904,
|
||||
"e": 0.9999999999999776
|
||||
},
|
||||
"uncertainty": {
|
||||
"x_mm": 3481.5587672468964,
|
||||
"y_mm": 1721.1687436205234,
|
||||
"z_mm": 230.0328185419544,
|
||||
"a_deg": 559.8212894814748,
|
||||
"b_deg": 10506.72751291286,
|
||||
"c_deg": 10506.727245747321,
|
||||
"e_mm": 105067.37089893252
|
||||
},
|
||||
"confidence": {
|
||||
"x": 6.278037366676565e-152,
|
||||
"y": 1.7807019001431863e-75,
|
||||
"z": 1.0228256841704706e-10,
|
||||
"a": 4.8671004844006695e-25,
|
||||
"b": 0.0,
|
||||
"c": 0.0,
|
||||
"e": 0.36787944117144233
|
||||
}
|
||||
},
|
||||
"statistics": {
|
||||
"observation_count": 40,
|
||||
"camera_count": 4,
|
||||
"marker_count": 23,
|
||||
"observed_marker_count": 21,
|
||||
"mean_detector_confidence": 0.46546541612214093,
|
||||
"mean_weighted_confidence": 0.42464863675174713,
|
||||
"mean_reprojection_error_px": 189.61546736162268,
|
||||
"quality_means": {
|
||||
"size": 0.7059842696189881,
|
||||
"aspect": 0.8417551598015706,
|
||||
"border": 0.8583333333333334,
|
||||
"homography": 0.8064378287982054
|
||||
},
|
||||
"quality_config": {
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2
|
||||
}
|
||||
}
|
||||
}
|
||||
|
Before Width: | Height: | Size: 1.3 MiB After Width: | Height: | Size: 1.3 MiB |
@@ -1,6 +1,6 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-28T21:28:20Z",
|
||||
"created_utc": "2026-05-28T21:44:42Z",
|
||||
"vision_config": {
|
||||
"MarkerType": "DICT_4X4_250",
|
||||
"MarkerSize": 0.025
|
||||
@@ -46,7 +46,7 @@
|
||||
},
|
||||
"detections": [
|
||||
{
|
||||
"observation_id": "492f34d0-5512-4bf1-9f5c-2e70826f2e71",
|
||||
"observation_id": "4d04662e-5080-4098-8fc4-46dce4e705f5",
|
||||
"type": "aruco",
|
||||
"marker_id": 102,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -100,7 +100,7 @@
|
||||
"confidence": 0.9494437061622057
|
||||
},
|
||||
{
|
||||
"observation_id": "f517029a-9c63-4593-a583-51bbc9991930",
|
||||
"observation_id": "449cb42f-7615-4568-b338-d2f044c6ec16",
|
||||
"type": "aruco",
|
||||
"marker_id": 243,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -154,7 +154,7 @@
|
||||
"confidence": 0.8899728634608616
|
||||
},
|
||||
{
|
||||
"observation_id": "7f05752e-d8d5-48c1-a9de-1d9fe78d34fc",
|
||||
"observation_id": "fc29ae3d-bcba-4cca-9d8b-e23fbe32ead8",
|
||||
"type": "aruco",
|
||||
"marker_id": 210,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -208,7 +208,7 @@
|
||||
"confidence": 0.5673043275049208
|
||||
},
|
||||
{
|
||||
"observation_id": "0f6019cf-ae79-4c58-b42f-454ad8a49b2c",
|
||||
"observation_id": "2c78068c-dce5-4783-af57-f452515adda2",
|
||||
"type": "aruco",
|
||||
"marker_id": 247,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -262,7 +262,7 @@
|
||||
"confidence": 0.8796777163094818
|
||||
},
|
||||
{
|
||||
"observation_id": "b6afa073-1316-4e40-b067-4ae58fa88bca",
|
||||
"observation_id": "bab2a129-54ce-4ea2-b1d7-23977a28b351",
|
||||
"type": "aruco",
|
||||
"marker_id": 246,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -316,7 +316,7 @@
|
||||
"confidence": 0.8015179238063419
|
||||
},
|
||||
{
|
||||
"observation_id": "5b95720f-f38f-4b70-b5a7-2a8407410850",
|
||||
"observation_id": "bb69b260-1543-4e95-80af-b6fee95585aa",
|
||||
"type": "aruco",
|
||||
"marker_id": 101,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -370,7 +370,7 @@
|
||||
"confidence": 0.3325004465415643
|
||||
},
|
||||
{
|
||||
"observation_id": "7dc93dc7-2686-48f6-bae4-c32d18d8a7a8",
|
||||
"observation_id": "e5ba9b93-f275-43e5-ab66-382210ce90d8",
|
||||
"type": "aruco",
|
||||
"marker_id": 215,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -424,7 +424,7 @@
|
||||
"confidence": 0.7952557920267838
|
||||
},
|
||||
{
|
||||
"observation_id": "94d6d6ac-b481-4d2d-a193-1451736ecf89",
|
||||
"observation_id": "4ad2b53e-2dbe-450c-92c7-d06b071cd796",
|
||||
"type": "aruco",
|
||||
"marker_id": 124,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -478,7 +478,7 @@
|
||||
"confidence": 0.34091855704160245
|
||||
},
|
||||
{
|
||||
"observation_id": "7da58bb6-8abb-4cb8-b93a-f19e3b30bab5",
|
||||
"observation_id": "7989f10c-2d6a-44b7-b16d-e721db2e585e",
|
||||
"type": "aruco",
|
||||
"marker_id": 229,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -532,7 +532,7 @@
|
||||
"confidence": 0.26810902547334975
|
||||
},
|
||||
{
|
||||
"observation_id": "c1447225-65f9-4e8e-ac4a-9913fbd3fa4a",
|
||||
"observation_id": "e2db913d-c818-4637-af5e-253d617ca04e",
|
||||
"type": "aruco",
|
||||
"marker_id": 122,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -586,7 +586,7 @@
|
||||
"confidence": 0.18665602922528673
|
||||
},
|
||||
{
|
||||
"observation_id": "c6bf9790-b7cd-4781-addf-593b43be010a",
|
||||
"observation_id": "76503575-cf6f-4ee8-9b3a-311ed41780be",
|
||||
"type": "aruco",
|
||||
"marker_id": 198,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -640,7 +640,7 @@
|
||||
"confidence": 0.20150745250271476
|
||||
},
|
||||
{
|
||||
"observation_id": "ea51fd97-3cc1-450e-8f88-341da29d24fd",
|
||||
"observation_id": "5458000b-97dc-419e-bc20-2cace8792227",
|
||||
"type": "aruco",
|
||||
"marker_id": 211,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -694,7 +694,7 @@
|
||||
"confidence": 0.6412317666518965
|
||||
},
|
||||
{
|
||||
"observation_id": "0f91cfb9-9382-456b-9fe7-2cc966a1fc59",
|
||||
"observation_id": "27ef8edf-4bf1-45cc-80d2-4ae6afc967c8",
|
||||
"type": "aruco",
|
||||
"marker_id": 208,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -748,7 +748,7 @@
|
||||
"confidence": 0.6280424706698967
|
||||
},
|
||||
{
|
||||
"observation_id": "bc4bcbaf-41ff-477e-a495-f1682dcf7c10",
|
||||
"observation_id": "26da9259-8af1-4dc7-b0dc-9f0bf85958c9",
|
||||
"type": "aruco",
|
||||
"marker_id": 217,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -802,7 +802,7 @@
|
||||
"confidence": 0.35596573288593486
|
||||
},
|
||||
{
|
||||
"observation_id": "a11c0669-5498-40dd-ab5d-944be3d29f37",
|
||||
"observation_id": "0caa0146-c0f8-4aaf-b394-95b32436dd0d",
|
||||
"type": "aruco",
|
||||
"marker_id": 206,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -856,7 +856,7 @@
|
||||
"confidence": 0.33820423087105295
|
||||
},
|
||||
{
|
||||
"observation_id": "d0910683-77ca-4ce7-9533-6cffa7f371ab",
|
||||
"observation_id": "0b3c110f-307c-48f6-9012-656bbc3cd9bf",
|
||||
"type": "aruco",
|
||||
"marker_id": 205,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -910,7 +910,7 @@
|
||||
"confidence": 0.28724459014346065
|
||||
},
|
||||
{
|
||||
"observation_id": "4b7bdee0-6a52-452c-b260-7eea65ad36c2",
|
||||
"observation_id": "698b3a77-1475-47ff-a9e7-c14d8240d428",
|
||||
"type": "aruco",
|
||||
"marker_id": 207,
|
||||
"marker_size_m": 0.025,
|
||||
|
Before Width: | Height: | Size: 1.1 MiB After Width: | Height: | Size: 1.1 MiB |
@@ -1,6 +1,6 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-28T21:28:21Z",
|
||||
"created_utc": "2026-05-28T21:44:43Z",
|
||||
"vision_config": {
|
||||
"MarkerType": "DICT_4X4_250",
|
||||
"MarkerSize": 0.025
|
||||
@@ -46,7 +46,7 @@
|
||||
},
|
||||
"detections": [
|
||||
{
|
||||
"observation_id": "0c8f6caa-0447-4f7c-99c4-2d30e31d30d6",
|
||||
"observation_id": "0ebc83d9-bb33-40a7-943b-78666c577574",
|
||||
"type": "aruco",
|
||||
"marker_id": 102,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -100,7 +100,7 @@
|
||||
"confidence": 0.8913851020933449
|
||||
},
|
||||
{
|
||||
"observation_id": "2df4a132-33c5-4ebf-b29f-dcc52478335b",
|
||||
"observation_id": "3e2026ef-788c-4f2c-9d0e-3bb4a4098c49",
|
||||
"type": "aruco",
|
||||
"marker_id": 124,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -154,7 +154,7 @@
|
||||
"confidence": 0.4804167810936165
|
||||
},
|
||||
{
|
||||
"observation_id": "ba6b5ed9-6a18-47e7-a843-af281c19c30f",
|
||||
"observation_id": "ebb20469-1b13-413a-8ea1-a35701ba2bd4",
|
||||
"type": "aruco",
|
||||
"marker_id": 243,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -208,7 +208,7 @@
|
||||
"confidence": 0.9132502955127223
|
||||
},
|
||||
{
|
||||
"observation_id": "f27e2575-2b8b-497b-be0a-b993197fd894",
|
||||
"observation_id": "211ff361-5d5c-4822-88b6-c5cec573090d",
|
||||
"type": "aruco",
|
||||
"marker_id": 122,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -262,7 +262,7 @@
|
||||
"confidence": 0.9456847621099602
|
||||
},
|
||||
{
|
||||
"observation_id": "a2356407-9a0c-460e-a2af-812ffddaddbf",
|
||||
"observation_id": "45c8a29e-1471-4fb7-bffc-27bdeb579be1",
|
||||
"type": "aruco",
|
||||
"marker_id": 247,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -316,7 +316,7 @@
|
||||
"confidence": 0.7448005120854795
|
||||
},
|
||||
{
|
||||
"observation_id": "e37d03aa-9079-465a-8278-e568958327fc",
|
||||
"observation_id": "16d87417-4ffe-4ce0-b51f-1e3fe5c9695e",
|
||||
"type": "aruco",
|
||||
"marker_id": 246,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -370,7 +370,7 @@
|
||||
"confidence": 0.7581700566520715
|
||||
},
|
||||
{
|
||||
"observation_id": "64462f28-959c-436f-9e7a-7b7e2c180bb7",
|
||||
"observation_id": "8c33c157-e141-4212-9279-69ad56760f53",
|
||||
"type": "aruco",
|
||||
"marker_id": 215,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -424,7 +424,7 @@
|
||||
"confidence": 0.826449608683203
|
||||
},
|
||||
{
|
||||
"observation_id": "23c3a60a-e454-4ac1-9ba7-13505aa8ffb7",
|
||||
"observation_id": "fe48446c-7538-4bd8-8cec-e96c3e5148b9",
|
||||
"type": "aruco",
|
||||
"marker_id": 210,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -478,7 +478,7 @@
|
||||
"confidence": 0.7813266383036261
|
||||
},
|
||||
{
|
||||
"observation_id": "977958d8-7446-4234-ab37-604bb459f146",
|
||||
"observation_id": "31c12fb9-cb8b-4677-89dc-b24351a14990",
|
||||
"type": "aruco",
|
||||
"marker_id": 229,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -532,7 +532,7 @@
|
||||
"confidence": 0.18684041285479083
|
||||
},
|
||||
{
|
||||
"observation_id": "28ae43e2-3bd3-4829-87d0-e9cb9526e304",
|
||||
"observation_id": "a059901a-85cf-42e8-870f-c88813554c08",
|
||||
"type": "aruco",
|
||||
"marker_id": 198,
|
||||
"marker_size_m": 0.025,
|
||||
|
Before Width: | Height: | Size: 1.3 MiB After Width: | Height: | Size: 1.3 MiB |
@@ -1,6 +1,6 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-28T21:28:21Z",
|
||||
"created_utc": "2026-05-28T21:44:43Z",
|
||||
"vision_config": {
|
||||
"MarkerType": "DICT_4X4_250",
|
||||
"MarkerSize": 0.025
|
||||
@@ -46,7 +46,7 @@
|
||||
},
|
||||
"detections": [
|
||||
{
|
||||
"observation_id": "56c7970c-3fdc-437a-82dd-918ef861ac60",
|
||||
"observation_id": "b2febb1e-6f84-4da7-b222-dad6bb306cfc",
|
||||
"type": "aruco",
|
||||
"marker_id": 102,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -100,7 +100,7 @@
|
||||
"confidence": 0.7012635429952238
|
||||
},
|
||||
{
|
||||
"observation_id": "4166196c-2e75-4461-97c1-378b70b0754f",
|
||||
"observation_id": "faabcc45-d069-4b1e-bbf4-a12019902b8a",
|
||||
"type": "aruco",
|
||||
"marker_id": 122,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -154,7 +154,7 @@
|
||||
"confidence": 0.5749326438029929
|
||||
},
|
||||
{
|
||||
"observation_id": "1e515a3c-1477-4fc5-b2e9-4a5c759201dc",
|
||||
"observation_id": "0773bfb1-9c4f-4add-9a74-bca81c38b2e8",
|
||||
"type": "aruco",
|
||||
"marker_id": 243,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -208,7 +208,7 @@
|
||||
"confidence": 0.9491420540234864
|
||||
},
|
||||
{
|
||||
"observation_id": "1549938d-da8e-4883-9239-92b88fc508c9",
|
||||
"observation_id": "abf720ab-c1d5-40c7-ad4f-9f58f8bd6ee1",
|
||||
"type": "aruco",
|
||||
"marker_id": 246,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -262,7 +262,7 @@
|
||||
"confidence": 0.4780463267147134
|
||||
},
|
||||
{
|
||||
"observation_id": "634d2ef2-2e64-4009-8d92-c16368d1b20a",
|
||||
"observation_id": "13752012-4478-43b7-b2f9-eab237d61864",
|
||||
"type": "aruco",
|
||||
"marker_id": 247,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -316,7 +316,7 @@
|
||||
"confidence": 0.5106218488640142
|
||||
},
|
||||
{
|
||||
"observation_id": "e96b7926-73c9-42c3-9c6e-e7bd66a0b4b7",
|
||||
"observation_id": "823b9adb-31fb-414d-aaaa-552432206d5c",
|
||||
"type": "aruco",
|
||||
"marker_id": 214,
|
||||
"marker_size_m": 0.025,
|
||||
@@ -370,7 +370,7 @@
|
||||
"confidence": 0.6041252446922665
|
||||
},
|
||||
{
|
||||
"observation_id": "c57683ff-d5c4-412c-b893-95d36298186f",
|
||||
"observation_id": "a33bdbe4-4e7d-42da-9c9e-6b5eaf4b5d8f",
|
||||
"type": "aruco",
|
||||
"marker_id": 210,
|
||||
"marker_size_m": 0.025,
|
||||
BIN
pipeline/2_Multiview_Trial/render_1d.png
Normal file
BIN
pipeline/2_Multiview_Trial/render_1d.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 MiB |
2316
pipeline/2_Multiview_Trial/render_1d_aruco_detection.json
Normal file
2316
pipeline/2_Multiview_Trial/render_1d_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
pipeline/2_Multiview_Trial/render_2a.png
Normal file
BIN
pipeline/2_Multiview_Trial/render_2a.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
15582
pipeline/2_Multiview_Trial/render_2a_aruco_detection.json
Normal file
15582
pipeline/2_Multiview_Trial/render_2a_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
pipeline/2_Multiview_Trial/render_2b.png
Normal file
BIN
pipeline/2_Multiview_Trial/render_2b.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.3 MiB |
22195
pipeline/2_Multiview_Trial/render_2b_aruco_detection.json
Normal file
22195
pipeline/2_Multiview_Trial/render_2b_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
BIN
pipeline/2_Multiview_Trial/render_2c.png
Normal file
BIN
pipeline/2_Multiview_Trial/render_2c.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 MiB |
2487
pipeline/2_Multiview_Trial/render_2c_aruco_detection.json
Normal file
2487
pipeline/2_Multiview_Trial/render_2c_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
12150
pipeline/2_Multiview_Trial/render_2d_aruco_detection.json
Normal file
12150
pipeline/2_Multiview_Trial/render_2d_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
462
pipeline/2_Multiview_Trial/robot_1_control.json
Normal file
462
pipeline/2_Multiview_Trial/robot_1_control.json
Normal file
@@ -0,0 +1,462 @@
|
||||
{
|
||||
"coordinateSystem": {
|
||||
"handedness": "right",
|
||||
"x": "right",
|
||||
"y": "backward",
|
||||
"z": "up"
|
||||
},
|
||||
"units": {
|
||||
"length": "mm",
|
||||
"rotation": "degree"
|
||||
},
|
||||
"vision_config": {
|
||||
"MarkerType": "DICT_4X4_250",
|
||||
"MarkerSize": 0.025
|
||||
},
|
||||
"renderingInfo": {
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
"cameraPosition": [-200, -900, 200],
|
||||
"cameraTarget": [210, -180, 180],
|
||||
"cameraUpVector": [0, 0, 1],
|
||||
"lightPosition": [-500, -500, 500],
|
||||
"lightTarget": [0, 0, 0],
|
||||
"lightUpVector": [0, 0, 1],
|
||||
"metric": "mm",
|
||||
"showSkeleton": true,
|
||||
"showMarkers": true,
|
||||
"backgroundColor": [0.70, 0.85, 1.0],
|
||||
"backgroundStrength": 0.20,
|
||||
"sunEnergy": 0.35,
|
||||
"areaEnergy": 120,
|
||||
"exposure": -1.5,
|
||||
"materials": {
|
||||
"wood": {
|
||||
"baseColor": [0.72, 0.52, 0.33],
|
||||
"roughness": 0.85,
|
||||
"metallic": 0.0
|
||||
},
|
||||
"plaWhite": {
|
||||
"baseColor": [0.95, 0.95, 0.95],
|
||||
"roughness": 0.45,
|
||||
"metallic": 0.0
|
||||
},
|
||||
"steel": {
|
||||
"baseColor": [0.72, 0.72, 0.75],
|
||||
"roughness": 0.25,
|
||||
"metallic": 1.0
|
||||
},
|
||||
"powderCoatBlue": {
|
||||
"baseColor": [0.15, 0.25, 0.7],
|
||||
"roughness": 0.55,
|
||||
"metallic": 0.0
|
||||
},
|
||||
"defaultPlastic": {
|
||||
"baseColor": [0.95, 0.95, 0.95],
|
||||
"roughness": 0.4,
|
||||
"metallic": 0.0
|
||||
},
|
||||
"skeletonRed": {
|
||||
"baseColor": [0.85, 0.20, 0.20],
|
||||
"roughness": 0.35,
|
||||
"metallic": 0.0
|
||||
},
|
||||
"markerBlack": {
|
||||
"baseColor": [0.04, 0.04, 0.04],
|
||||
"roughness": 0.80,
|
||||
"metallic": 0.0
|
||||
}
|
||||
},
|
||||
"skeletonDefaults": {
|
||||
"radius": 4,
|
||||
"color": [0.85, 0.20, 0.20]
|
||||
},
|
||||
"markerDefaults": {
|
||||
"size": 25,
|
||||
"thickness": 1,
|
||||
"color": [0.04, 0.04, 0.04]
|
||||
}
|
||||
},
|
||||
"defaultPosition": {
|
||||
"x": 100,
|
||||
"y": 30,
|
||||
"z": -30,
|
||||
"a": -120,
|
||||
"b": 22,
|
||||
"c": 91,
|
||||
"e": 10
|
||||
},
|
||||
"recognized": {
|
||||
"x": null,
|
||||
"y": null,
|
||||
"z": null,
|
||||
"a": null,
|
||||
"b": null,
|
||||
"c": null,
|
||||
"e": null
|
||||
},
|
||||
"multiview_calculation": {
|
||||
"combine_mode": "mean",
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2,
|
||||
"normal_visibility_factor": 0.01,
|
||||
"spin_factor": 0.3,
|
||||
"weight_floor": 0.3
|
||||
},
|
||||
"movements": {
|
||||
"x": null,
|
||||
"y": null,
|
||||
"z": null,
|
||||
"a": null,
|
||||
"b": null,
|
||||
"c": null,
|
||||
"e": null
|
||||
},
|
||||
"links": {
|
||||
"Board": {
|
||||
"parent": null,
|
||||
"size": [1000, 200, 25],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"skeleton": {
|
||||
"from": [0, 0, 16],
|
||||
"to": [1000, 0, 16],
|
||||
"radius": 4,
|
||||
"color": [0.85, 0.20, 0.20]
|
||||
},
|
||||
"markers":[
|
||||
{"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]},
|
||||
{"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]},
|
||||
{"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]},
|
||||
{"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]},
|
||||
{"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]},
|
||||
{"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]},
|
||||
{"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]},
|
||||
{"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]},
|
||||
{"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Board.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "wood"
|
||||
},
|
||||
{
|
||||
"stlFile": "surfaces/BoardRail.stl",
|
||||
"originOfModel": [0, 0, 0],
|
||||
"rotationOfModelDegree": [0, 0, -90],
|
||||
"material": "steel"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Base": {
|
||||
"parent": "Board",
|
||||
"size": [150, 200, 150],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, 0, 16],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "x"
|
||||
},
|
||||
"skeleton": {
|
||||
"from": [0, 108, 45],
|
||||
"to": [110, 108, 45],
|
||||
"radius": 4,
|
||||
"color": [0.20, 0.80, 0.20]
|
||||
},
|
||||
"markers": [
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Base.stl",
|
||||
"originOfModel": [-30, 0, -35],
|
||||
"rotationOfModelDegree": [0, 0, 0],
|
||||
"material": "plaWhite"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Arm1": {
|
||||
"parent": "Base",
|
||||
"size": [70, 250, 70],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Joint1",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [110, 108, 45],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "y"
|
||||
},
|
||||
"skeleton": {
|
||||
"from": [0, 0, 0],
|
||||
"to": [0, -250, 0],
|
||||
"radius": 4,
|
||||
"color": [0.20, 0.20, 0.90]
|
||||
},
|
||||
"markers": [
|
||||
{
|
||||
"id": 198,
|
||||
"name": "aruco_198",
|
||||
"position": [0, -160, 35],
|
||||
"normal": [0, 0, 1],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
},
|
||||
{
|
||||
"id": 229,
|
||||
"name": "aruco_229",
|
||||
"position": [0, -250, 35],
|
||||
"normal": [0, 0, 1],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
},
|
||||
{
|
||||
"id": 242,
|
||||
"name": "aruco_242",
|
||||
"position": [0, -250, -35],
|
||||
"normal": [0, 0, -1],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
},
|
||||
{
|
||||
"id": 243,
|
||||
"name": "aruco_243",
|
||||
"position": [0, -285, 0],
|
||||
"normal": [0, -1, 0],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
}
|
||||
],
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Holm.stl",
|
||||
"originOfModel__": [-25,29,-28.5],
|
||||
"originOfModel": [-29,25,28.5],
|
||||
"rotationOfModelDegree__": [0, 0, 0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "powderCoatBlue"
|
||||
}
|
||||
]
|
||||
},
|
||||
"Ellbow": {
|
||||
"parent": "Arm1",
|
||||
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
|
||||
"jointToParent": {
|
||||
"name": "Joint2",
|
||||
"type": "revolute",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "z"
|
||||
},
|
||||
|
||||
"skeleton": {
|
||||
"from": [0, 0, 0],
|
||||
"to": [90, 0, 0],
|
||||
"radius": 4,
|
||||
"color": [0.90, 0.20, 0.20]
|
||||
},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Ellebogen.stl",
|
||||
"originOfModel": [90,0,0],
|
||||
"rotationOfModelDegree": [0,-90,-90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
{
|
||||
"id": 244,
|
||||
"name": "aruco_244",
|
||||
"position": [125, 0, 0],
|
||||
"normal": [1, 0, 0],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
},
|
||||
{
|
||||
"id": 245,
|
||||
"name": "aruco_245",
|
||||
"position": [90, 0, -35],
|
||||
"normal": [0, 0, -1],
|
||||
"size": 25,
|
||||
"spin": 0
|
||||
},
|
||||
{
|
||||
"id": 246,
|
||||
"name": "aruco_246",
|
||||
"position": [90, 0, 35],
|
||||
"normal": [0, 0, 1],
|
||||
"size": 25
|
||||
},
|
||||
{
|
||||
"id": 247,
|
||||
"name": "aruco_247",
|
||||
"position": [52.5, 0, 35],
|
||||
"normal": [0, 0, 1],
|
||||
"size": 25
|
||||
}
|
||||
]
|
||||
|
||||
},
|
||||
"Arm2": {
|
||||
"parent": "Ellbow",
|
||||
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [90, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "a"
|
||||
},
|
||||
|
||||
"skeleton": {
|
||||
"from": [0, 0, 0],
|
||||
"to": [0, -250, 0],
|
||||
"radius": 4,
|
||||
"color": [0.95, 0.85, 0.20]
|
||||
},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Unterarm.stl",
|
||||
"originOfModel": [0,-250,0],
|
||||
"rotationOfModelDegree": [180, 0, -90],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
],
|
||||
"markers":[
|
||||
|
||||
{"id":124, "position":[24.75, -112, -24.75],"normal":[1,0,-1]},
|
||||
{"id":122, "name": "aruco_122", "position":[-35,-112,0], "normal":[-1,0,0]},
|
||||
{"id":218, "name": "aruco_218", "position":[35,-112,0], "normal":[1,0,0]},
|
||||
{"id":122, "name": "aruco_122", "position":[0, -182, 30],"normal":[0,0,1]},
|
||||
{"id":101, "name": "aruco_122", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]},
|
||||
{"id":102, "name": "aruco_122", "position":[-24.75, -182, -24.75],"normal":[-1,0,-1]},
|
||||
{"id":124, "name": "aruco_124", "position":[-35,-219,0], "normal":[-1,0,0]},
|
||||
{"id":219, "name": "aruco_219", "position":[35,-219,0], "normal":[1,0,0]}
|
||||
|
||||
]
|
||||
},
|
||||
"Hand": {
|
||||
"parent": "Arm2",
|
||||
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
|
||||
"jointToParent": {
|
||||
"name": "Joint4",
|
||||
"type": "revolute",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [0, -250, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "b"
|
||||
},
|
||||
|
||||
"skeleton": {
|
||||
"from": [0, 0, 0],
|
||||
"to": [0, -35, 0],
|
||||
"radius": 4,
|
||||
"color": [0.95, 0.55, 0.15]
|
||||
}
|
||||
},
|
||||
"Palm": {
|
||||
"parent": "Hand",
|
||||
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
|
||||
"jointToParent": {
|
||||
"name": "Joint3",
|
||||
"type": "revolute",
|
||||
"axis": [0, -1, 0],
|
||||
"origin": [0, 0, 0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "c"
|
||||
},
|
||||
|
||||
"skeleton": {
|
||||
"from": [-50, -35, 0],
|
||||
"to": [50, -35, 0],
|
||||
"radius": 7,
|
||||
"color": [0.95, 0.20, 0.20]
|
||||
}
|
||||
},
|
||||
"FingerA": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [1, 0, 0],
|
||||
"origin": [4, -35,0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {
|
||||
"from": [0, 0,0],
|
||||
"to": [0, -60, 0],
|
||||
"radius": 4,
|
||||
"color": [0.20, 0.80, 0.20]
|
||||
},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [24,0,-9.1],
|
||||
"rotationOfModelDegree": [90, -90,0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
},
|
||||
"FingerB": {
|
||||
"parent": "Palm",
|
||||
"size": [80, 60, 20],
|
||||
"mountPosition": [0, 0, 0],
|
||||
"mountRotation": [0, 0, 0],
|
||||
"jointToParent": {
|
||||
"name": "Slider",
|
||||
"type": "linear",
|
||||
"axis": [-1, 0, 0],
|
||||
"origin": [-4, -35,0],
|
||||
"rotation": [0, 0, 0],
|
||||
"variable": "e"
|
||||
},
|
||||
"skeleton": {
|
||||
"from": [0, 0,0],
|
||||
"to": [0, -60, 0],
|
||||
"radius": 4,
|
||||
"color": [0.20, 0.80, 0.20]
|
||||
},
|
||||
"model": [
|
||||
{
|
||||
"stlFile": "surfaces/Finger.stl",
|
||||
"originOfModel": [-24,0,9.1],
|
||||
"rotationOfModelDegree": [90, 90,0],
|
||||
"material": "defaultPlastic"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2,5 +2,6 @@
|
||||
python3 1_detect_aruco_observations.py --image render_1a.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_1b.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_1c.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_1d.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
|
||||
python3 2_Multiview.py --robot ../robot.json --detections render_1a_aruco_detection.json render_1b_aruco_detection.json render_1c_aruco_detection.json --outDir . --write-summary --max-iter 120
|
||||
python3 2_Multiview.py --robot ../robot.json --detections render_1a_aruco_detection.json render_1b_aruco_detection.json render_1c_aruco_detection.json render_1d_aruco_detection.json --outDir . --write-summary --max-iter 120
|
||||
7
pipeline/2_Multiview_Trial/run_pipeline_2.bat
Normal file
7
pipeline/2_Multiview_Trial/run_pipeline_2.bat
Normal file
@@ -0,0 +1,7 @@
|
||||
|
||||
python3 1_detect_aruco_observations.py --image render_2a.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_2b.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_2c.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_2d.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
|
||||
python3 2_Multiview.py --robot ../robot.json --detections render_2a_aruco_detection.json render_2b_aruco_detection.json render_2c_aruco_detection.json render_2d_aruco_detection.json --outDir . --write-summary --max-iter 120
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,787 +0,0 @@
|
||||
#!/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()
|
||||
@@ -1,706 +0,0 @@
|
||||
#!/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"]
|
||||
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 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()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,971 +0,0 @@
|
||||
#!/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)
|
||||
|
||||
|
||||
def compute_soft_constraint_residuals(
|
||||
robot_state: Dict[str, float],
|
||||
robot_markers: Dict[int, Dict[str, Any]],
|
||||
link_transforms: Dict[str, np.ndarray],
|
||||
robot: Dict[str, Any],
|
||||
enabled_constraints: Dict[str, ConstraintResult]
|
||||
) -> List[float]:
|
||||
"""
|
||||
Compute residuals from soft constraints (kinematic consistency, rigid body distances).
|
||||
Returns a list of constraint residuals to append to the total residual vector.
|
||||
"""
|
||||
residuals = []
|
||||
weight_scale = 0.1 # Weight for soft constraints relative to reprojection errors
|
||||
|
||||
# Constraint 1: Rigid body distances within each link
|
||||
if enabled_constraints['RigidBodyDistances'].enabled:
|
||||
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:
|
||||
continue
|
||||
|
||||
# Compute all pairwise distances in world coords
|
||||
for i in range(len(link_markers)):
|
||||
for j in range(i + 1, len(link_markers)):
|
||||
m_i = link_markers[i]
|
||||
m_j = link_markers[j]
|
||||
|
||||
pos_i = compute_marker_world_position(m_i, link_transforms)
|
||||
pos_j = compute_marker_world_position(m_j, link_transforms)
|
||||
|
||||
dist_world = np.linalg.norm(pos_i - pos_j)
|
||||
|
||||
# Reference distance in local coords
|
||||
dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m'])
|
||||
|
||||
# Residual: difference should be zero (rigid body)
|
||||
error = dist_world - dist_local
|
||||
residuals.append(error * weight_scale * 0.1) # Very soft weight
|
||||
|
||||
# Constraint 2: Fixed X-distances between links (Arm1 <-> Ellbow)
|
||||
if enabled_constraints['InterLinkXDistances'].enabled:
|
||||
arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1']
|
||||
ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow']
|
||||
|
||||
if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1:
|
||||
# Get first marker from each link
|
||||
m_arm1 = arm1_markers[0]
|
||||
m_ellbow = ellbow_markers[0]
|
||||
|
||||
pos_arm1 = compute_marker_world_position(m_arm1, link_transforms)
|
||||
pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms)
|
||||
|
||||
# X-distance in world should match reference (relative position)
|
||||
# Since both rotate around X-axis at different points, we check consistency
|
||||
x_diff_world = pos_ellbow[0] - pos_arm1[0]
|
||||
x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0]
|
||||
|
||||
error = x_diff_world - x_diff_ref
|
||||
residuals.append(error * weight_scale)
|
||||
|
||||
return residuals
|
||||
|
||||
|
||||
def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray:
|
||||
"""Compute the world position of a marker given current link transforms."""
|
||||
link_transform = link_transforms[marker['link_name']]
|
||||
local_pos = np.concatenate([marker['position_m'], [1.0]])
|
||||
world_pos = (link_transform @ local_pos)[:3]
|
||||
return world_pos
|
||||
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 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],
|
||||
enabled_constraints: Dict[str, ConstraintResult]
|
||||
) -> np.ndarray:
|
||||
robot_state, camera_params = unpack_parameters(params, len(views))
|
||||
link_transforms = compute_link_transforms(robot, robot_state, scale)
|
||||
|
||||
residuals = []
|
||||
|
||||
# Reprojection residuals (primary observation)
|
||||
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))
|
||||
|
||||
# Weak priors on robot state
|
||||
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)
|
||||
|
||||
# Soft constraints (kinematic consistency, rigid body constraints)
|
||||
soft_constraint_residuals = compute_soft_constraint_residuals(
|
||||
robot_state, robot_markers, link_transforms, robot, enabled_constraints
|
||||
)
|
||||
residuals.extend(soft_constraint_residuals)
|
||||
|
||||
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))
|
||||
|
||||
|
||||
def print_constraint_sanity_check(
|
||||
robot_state: Dict[str, float],
|
||||
robot_markers: Dict[int, Dict[str, Any]],
|
||||
link_transforms: Dict[str, np.ndarray],
|
||||
robot: Dict[str, Any],
|
||||
enabled_constraints: Dict[str, ConstraintResult]
|
||||
) -> None:
|
||||
"""
|
||||
Print sanity checks for all constraints to verify the optimization result.
|
||||
"""
|
||||
print("\n" + "=" * 70)
|
||||
print("CONSTRAINT SANITY CHECKS (after optimization)")
|
||||
print("=" * 70)
|
||||
|
||||
# Check 1: Rigid body distances
|
||||
if enabled_constraints['RigidBodyDistances'].enabled:
|
||||
print("\n1. RIGID BODY DISTANCES")
|
||||
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:
|
||||
continue
|
||||
|
||||
max_error = 0.0
|
||||
for i in range(len(link_markers)):
|
||||
for j in range(i + 1, len(link_markers)):
|
||||
m_i = link_markers[i]
|
||||
m_j = link_markers[j]
|
||||
|
||||
pos_i = compute_marker_world_position(m_i, link_transforms)
|
||||
pos_j = compute_marker_world_position(m_j, link_transforms)
|
||||
|
||||
dist_world = np.linalg.norm(pos_i - pos_j)
|
||||
dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m'])
|
||||
error = abs(dist_world - dist_local)
|
||||
max_error = max(max_error, error)
|
||||
|
||||
status = "✓" if max_error < 1.0 else "⚠" if max_error < 5.0 else "✗"
|
||||
print(f" {link_name:10s}: max_error = {max_error:.3f} mm {status}")
|
||||
|
||||
# Check 2: Inter-link X distances
|
||||
if enabled_constraints['InterLinkXDistances'].enabled:
|
||||
print("\n2. INTER-LINK X-DISTANCES")
|
||||
arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1']
|
||||
ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow']
|
||||
|
||||
if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1:
|
||||
m_arm1 = arm1_markers[0]
|
||||
m_ellbow = ellbow_markers[0]
|
||||
|
||||
pos_arm1 = compute_marker_world_position(m_arm1, link_transforms)
|
||||
pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms)
|
||||
|
||||
x_diff_world = pos_ellbow[0] - pos_arm1[0]
|
||||
x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0]
|
||||
error = abs(x_diff_world - x_diff_ref)
|
||||
|
||||
status = "✓" if error < 1.0 else "⚠" if error < 5.0 else "✗"
|
||||
print(f" Arm1 <-> Ellbow: error = {error:.3f} mm {status}")
|
||||
|
||||
# Check 3: Arm2 sin(a) dependency
|
||||
if enabled_constraints['Arm2SinADependency'].enabled:
|
||||
print("\n3. ARM2 sin(a) DEPENDENCY (sanity check)")
|
||||
arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2']
|
||||
if len(arm2_markers) >= 2:
|
||||
# Check that markers with different Z values have different X spreads
|
||||
a_rad = math.radians(robot_state['a'])
|
||||
sin_a = math.sin(a_rad)
|
||||
cos_a = math.cos(a_rad)
|
||||
|
||||
z_variations = {}
|
||||
for m in arm2_markers:
|
||||
z_local = m['position_m'][2]
|
||||
x_local = m['position_m'][0]
|
||||
pos_world = compute_marker_world_position(m, link_transforms)
|
||||
x_world = pos_world[0]
|
||||
|
||||
# Expected: x_world = 90 + x_local * cos(a) - z_local * sin(a)
|
||||
x_expected = 90 * (robot.get('renderingInfo', {}).get('metric', 'mm') == 'mm' and 0.09 or 0.09) + x_local * cos_a - z_local * sin_a
|
||||
x_error = abs(x_world - x_expected)
|
||||
|
||||
if z_local not in z_variations:
|
||||
z_variations[z_local] = []
|
||||
z_variations[z_local].append(x_error)
|
||||
|
||||
max_error = max(max(errors) for errors in z_variations.values()) if z_variations else 0.0
|
||||
status = "✓" if max_error < 5.0 else "⚠" if max_error < 10.0 else "⚠"
|
||||
print(f" X-consistency with sin(a): max_error = {max_error:.3f} mm {status}")
|
||||
print(f" (Note: this is a consistency check, not a hard constraint)")
|
||||
|
||||
print("=" * 70)
|
||||
|
||||
|
||||
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)
|
||||
|
||||
# Validate constraints
|
||||
print("\n" + "=" * 70)
|
||||
print("CONSTRAINT VALIDATION")
|
||||
print("=" * 70)
|
||||
enabled_constraints = validate_constraints(robot, robot_markers)
|
||||
for constraint_name, result in enabled_constraints.items():
|
||||
print(result)
|
||||
print("=" * 70)
|
||||
|
||||
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, enabled_constraints)
|
||||
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, enabled_constraints),
|
||||
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))
|
||||
|
||||
# Print constraint sanity checks
|
||||
link_transforms = compute_link_transforms(robot, robot_state, scale)
|
||||
print_constraint_sanity_check(robot_state, robot_markers, link_transforms, robot, enabled_constraints)
|
||||
|
||||
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()
|
||||
827
pipeline/2_estimate_camera_from_observations.py
Normal file
827
pipeline/2_estimate_camera_from_observations.py
Normal file
@@ -0,0 +1,827 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
2_estimate_camera_from_observations.py
|
||||
|
||||
Estimate a single camera pose from ArUco observations stored in
|
||||
*_aruco_detection.json, using marker world positions from robot.json.
|
||||
|
||||
This follows the same mathematical idea as readTwoImages.py:
|
||||
1) use detected marker observations,
|
||||
2) get an initial pose from a rigid transform,
|
||||
3) refine with Levenberg-Marquardt on normalized reprojection residuals.
|
||||
|
||||
Difference to readTwoImages.py:
|
||||
- No image processing here.
|
||||
- Input is the observation JSON created by 1_detect_aruco_observations.py.
|
||||
- Output is xxx_camera_pose.json.
|
||||
- Unknown marker reconstruction is intentionally omitted.
|
||||
|
||||
Assumptions:
|
||||
- robot.json contains a marker list for the board/world frame.
|
||||
- At minimum, marker positions are present for the reference markers.
|
||||
- The detection JSON contains camera intrinsics and marker corners.
|
||||
|
||||
Typical usage:
|
||||
python3 2_estimate_camera_from_observations.py \
|
||||
-i frame_0001_aruco_detection.json \
|
||||
-robot robot.json \
|
||||
-outDir results/
|
||||
|
||||
Output:
|
||||
frame_0001_camera_pose.json
|
||||
|
||||
Notes on uncertainty:
|
||||
- The script computes an approximate 6x6 covariance for the pose parameters
|
||||
[rvec_x, rvec_y, rvec_z, t_x, t_y, t_z].
|
||||
- It also propagates that covariance to camera center uncertainty in world
|
||||
coordinates and to approximate roll/pitch/yaw uncertainty.
|
||||
"""
|
||||
|
||||
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)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Intrinsics
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def load_intrinsics_from_detection(detection: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Primary source: the embedded camera intrinsics in the detection JSON.
|
||||
"""
|
||||
camera = detection.get("camera", {})
|
||||
K = camera.get("camera_matrix", None)
|
||||
D = camera.get("distortion_coefficients", None)
|
||||
|
||||
if K is None:
|
||||
raise KeyError("camera_matrix missing in detection JSON.")
|
||||
if D is None:
|
||||
D = [0, 0, 0, 0, 0]
|
||||
|
||||
K = np.array(K, dtype=np.float32).reshape(3, 3)
|
||||
D = np.array(D, dtype=np.float32).reshape(-1, 1)
|
||||
return K, D
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Robot JSON parsing
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def _rotation_matrix_from_any(rotation: Any) -> np.ndarray:
|
||||
"""
|
||||
Best-effort parser for marker rotation.
|
||||
|
||||
Supported inputs:
|
||||
- 3x3 matrix as nested list
|
||||
- flat 9 list
|
||||
- dict with keys:
|
||||
* rotation_matrix / matrix
|
||||
* rvec / rodriques / rodrigues
|
||||
* euler_deg / rpy_deg / roll_pitch_yaw_deg
|
||||
* euler_rad / rpy_rad / roll_pitch_yaw_rad
|
||||
* quaternion / quat (best-effort, expects [x,y,z,w] unless specified)
|
||||
- None => identity
|
||||
|
||||
The pose estimator below only needs marker positions, but we keep
|
||||
this parser for completeness and future extension.
|
||||
"""
|
||||
if rotation is None:
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
# Direct matrix
|
||||
if isinstance(rotation, (list, tuple, np.ndarray)):
|
||||
arr = np.array(rotation, dtype=np.float32)
|
||||
if arr.shape == (3, 3):
|
||||
return arr
|
||||
if arr.size == 9:
|
||||
return arr.reshape(3, 3).astype(np.float32)
|
||||
if arr.size == 3:
|
||||
# Treat as Rodrigues vector
|
||||
R, _ = cv2.Rodrigues(arr.reshape(3, 1))
|
||||
return R.astype(np.float32)
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
if isinstance(rotation, dict):
|
||||
for key in ("rotation_matrix", "matrix"):
|
||||
if key in rotation:
|
||||
return _rotation_matrix_from_any(rotation[key])
|
||||
|
||||
for key in ("rvec", "rodrigues", "rodriques"):
|
||||
if key in rotation:
|
||||
v = np.array(rotation[key], dtype=np.float32).reshape(3, 1)
|
||||
R, _ = cv2.Rodrigues(v)
|
||||
return R.astype(np.float32)
|
||||
|
||||
def euler_to_R(roll: float, pitch: float, yaw: float, degrees: bool = True) -> np.ndarray:
|
||||
if degrees:
|
||||
roll = np.deg2rad(roll)
|
||||
pitch = np.deg2rad(pitch)
|
||||
yaw = np.deg2rad(yaw)
|
||||
cr, sr = np.cos(roll), np.sin(roll)
|
||||
cp, sp = np.cos(pitch), np.sin(pitch)
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
|
||||
Rx = np.array([[1, 0, 0],
|
||||
[0, cr, -sr],
|
||||
[0, sr, cr]], dtype=np.float32)
|
||||
Ry = np.array([[cp, 0, sp],
|
||||
[0, 1, 0],
|
||||
[-sp, 0, cp]], dtype=np.float32)
|
||||
Rz = np.array([[cy, -sy, 0],
|
||||
[sy, cy, 0],
|
||||
[0, 0, 1]], dtype=np.float32)
|
||||
# ZYX convention
|
||||
return (Rz @ Ry @ Rx).astype(np.float32)
|
||||
|
||||
for key in ("euler_deg", "rpy_deg", "roll_pitch_yaw_deg"):
|
||||
if key in rotation:
|
||||
vals = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if vals.size == 3:
|
||||
return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=True)
|
||||
|
||||
for key in ("euler_rad", "rpy_rad", "roll_pitch_yaw_rad"):
|
||||
if key in rotation:
|
||||
vals = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if vals.size == 3:
|
||||
return euler_to_R(float(vals[0]), float(vals[1]), float(vals[2]), degrees=False)
|
||||
|
||||
for key in ("quaternion", "quat"):
|
||||
if key in rotation:
|
||||
q = np.array(rotation[key], dtype=np.float32).reshape(-1)
|
||||
if q.size == 4:
|
||||
# Best-effort: try [x,y,z,w]
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
R = np.array([
|
||||
[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
|
||||
[2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
|
||||
[2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
|
||||
], dtype=np.float32)
|
||||
return R
|
||||
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
|
||||
def get_marker_rotation(marker: Dict[str, Any]) -> np.ndarray:
|
||||
"""
|
||||
Flexible rotation extraction. Falls back to identity if absent.
|
||||
"""
|
||||
for key in ("rotation", "rotation_matrix", "matrix", "pose_rotation", "orientation"):
|
||||
if key in marker:
|
||||
return _rotation_matrix_from_any(marker[key])
|
||||
|
||||
# Also allow flat pose-style fields
|
||||
if "rvec" in marker or "rodrigues" in marker:
|
||||
return _rotation_matrix_from_any({"rvec": marker.get("rvec", marker.get("rodrigues"))})
|
||||
if "euler_deg" in marker:
|
||||
return _rotation_matrix_from_any({"euler_deg": marker["euler_deg"]})
|
||||
if "rpy_deg" in marker:
|
||||
return _rotation_matrix_from_any({"rpy_deg": marker["rpy_deg"]})
|
||||
if "quaternion" in marker:
|
||||
return _rotation_matrix_from_any({"quaternion": marker["quaternion"]})
|
||||
|
||||
return np.eye(3, dtype=np.float32)
|
||||
|
||||
|
||||
def load_marker_lookup(robot_json_path: str) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Supports the new format:
|
||||
robot_data["links"]["Board"]["markers"]
|
||||
|
||||
Fallback:
|
||||
robot_data["Marker"]
|
||||
"""
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
marker_lookup: Dict[int, Dict[str, Any]] = {}
|
||||
|
||||
links = robot_data.get("links", {})
|
||||
board = links.get("Board")
|
||||
|
||||
markers = None
|
||||
if board and "markers" in board:
|
||||
markers = board["markers"]
|
||||
|
||||
if not markers:
|
||||
markers = robot_data.get("Marker", [])
|
||||
|
||||
for marker in markers:
|
||||
marker_id = int(marker.get("id", -1))
|
||||
if marker_id < 0:
|
||||
continue
|
||||
|
||||
if "position" not in marker:
|
||||
continue
|
||||
|
||||
pos = marker.get("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),
|
||||
"rotation": rotation,
|
||||
"on": marker.get("on", "unknown"),
|
||||
}
|
||||
|
||||
return marker_lookup
|
||||
|
||||
|
||||
def load_robot_marker_size(robot_json_path: str) -> Optional[float]:
|
||||
"""
|
||||
Best-effort marker size reader from robot.json.
|
||||
Returns meters if found, otherwise None.
|
||||
"""
|
||||
robot_json_path = resolve_path(robot_json_path)
|
||||
with open(robot_json_path, "r", encoding="utf-8") as f:
|
||||
robot_data = json.load(f)
|
||||
|
||||
vision_config = robot_data.get("vision_config", {})
|
||||
size = vision_config.get("MarkerSize", None)
|
||||
if size is None:
|
||||
return None
|
||||
try:
|
||||
return float(size)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Geometry / pose helpers
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def marker_local_corners(marker_size_m: float) -> np.ndarray:
|
||||
half = marker_size_m / 2.0
|
||||
# Same corner order as the readTwoImages.py example
|
||||
return np.array([
|
||||
[-half, half, 0.0],
|
||||
[ half, half, 0.0],
|
||||
[ half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
], dtype=np.float32)
|
||||
|
||||
|
||||
def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
Find R, t such that B ≈ R A + t.
|
||||
A, B: Nx3
|
||||
"""
|
||||
assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3"
|
||||
N = A.shape[0]
|
||||
if N < 2:
|
||||
raise ValueError("Need at least 2 points; 3+ recommended.")
|
||||
|
||||
centroid_A = A.mean(axis=0)
|
||||
centroid_B = B.mean(axis=0)
|
||||
|
||||
AA = A - centroid_A
|
||||
BB = B - centroid_B
|
||||
|
||||
H = AA.T @ BB
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
if np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = centroid_B - R @ centroid_A
|
||||
return R.astype(np.float32), t.astype(np.float32)
|
||||
|
||||
|
||||
def undistort_to_normalized(points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray:
|
||||
pts = points_px.reshape(-1, 1, 2).astype(np.float32)
|
||||
und = cv2.undistortPoints(pts, K, D, P=None)
|
||||
return und.reshape(-1, 2).astype(np.float32)
|
||||
|
||||
|
||||
def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
|
||||
R, _ = cv2.Rodrigues(rvec.reshape(3, 1))
|
||||
return R.astype(np.float32)
|
||||
|
||||
|
||||
def R_to_euler_zyx(R: np.ndarray) -> Tuple[float, float, float]:
|
||||
"""
|
||||
Return roll, pitch, yaw in degrees using ZYX convention.
|
||||
"""
|
||||
yaw = float(np.degrees(np.arctan2(R[1, 0], R[0, 0])))
|
||||
sp = np.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2)
|
||||
pitch = float(np.degrees(np.arctan2(-R[2, 0], sp)))
|
||||
roll = float(np.degrees(np.arctan2(R[2, 1], R[2, 2])))
|
||||
return roll, pitch, yaw
|
||||
|
||||
|
||||
def theta_to_camera_pose(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""
|
||||
theta = [omega_x, omega_y, omega_z, t_x, t_y, t_z]
|
||||
Returns:
|
||||
R_wc, t_wc, camera_center_world
|
||||
"""
|
||||
omega = theta[0:3]
|
||||
t_wc = theta[3:6].reshape(3, 1).astype(np.float32)
|
||||
R_wc, _ = cv2.Rodrigues(omega.reshape(3, 1))
|
||||
R_wc = R_wc.astype(np.float32)
|
||||
R_cw = R_wc.T
|
||||
camera_center_world = (-R_cw @ t_wc).reshape(3)
|
||||
return R_wc, t_wc.reshape(3), camera_center_world
|
||||
|
||||
|
||||
def build_projection_matrix(K: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return K @ np.hstack([R, t.reshape(3, 1)])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# LM on normalized residuals (same style as readTwoImages.py)
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def pack_params(omega: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return np.hstack([omega.reshape(3), t.reshape(3)]).astype(np.float64)
|
||||
|
||||
|
||||
def unpack_params(theta: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
omega = theta[0:3]
|
||||
t = theta[3:6]
|
||||
return omega, t
|
||||
|
||||
|
||||
def residuals_centers_normalized(theta: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Residuals in normalized coordinates:
|
||||
obs_norm - project(R X_world + t)
|
||||
"""
|
||||
omega, t = unpack_params(theta)
|
||||
R_wc = cv2.Rodrigues(omega.reshape(3, 1))[0].astype(np.float64)
|
||||
X_cam = (R_wc @ X_world.T + t.reshape(3, 1)).T
|
||||
uv = X_cam[:, :2] / X_cam[:, 2:3]
|
||||
r = (obs_norm - uv).reshape(-1)
|
||||
return r
|
||||
|
||||
|
||||
def numerical_jacobian(f, theta: np.ndarray, eps: float, *args) -> Tuple[np.ndarray, np.ndarray]:
|
||||
r0 = f(theta, *args)
|
||||
m = r0.size
|
||||
n = theta.size
|
||||
J = np.zeros((m, n), dtype=np.float64)
|
||||
for k in range(n):
|
||||
th = theta.copy()
|
||||
th[k] += eps
|
||||
rk = f(th, *args)
|
||||
J[:, k] = (rk - r0) / eps
|
||||
return J, r0
|
||||
|
||||
|
||||
def lm_solve(theta0: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray,
|
||||
max_iter: int = 60,
|
||||
eps_jac: float = 1e-6,
|
||||
lambda_init: float = 1e-3) -> Tuple[np.ndarray, Dict[str, List[float]]]:
|
||||
lam = lambda_init
|
||||
theta = theta0.copy().astype(np.float64)
|
||||
history = {"iters": [], "rms": [], "lambda": []}
|
||||
|
||||
for it in range(max_iter):
|
||||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm)
|
||||
rms = float(np.sqrt(np.mean(r * r))) if r.size else 0.0
|
||||
history["iters"].append(it)
|
||||
history["rms"].append(rms)
|
||||
history["lambda"].append(lam)
|
||||
|
||||
JTJ = J.T @ J
|
||||
g = J.T @ r
|
||||
H = JTJ + lam * np.eye(JTJ.shape[0], dtype=np.float64)
|
||||
|
||||
try:
|
||||
delta = -np.linalg.solve(H, g)
|
||||
except np.linalg.LinAlgError:
|
||||
delta, *_ = np.linalg.lstsq(H, -g, rcond=None)
|
||||
|
||||
theta_trial = theta + delta
|
||||
r_trial = residuals_centers_normalized(theta_trial, X_world, obs_norm)
|
||||
rms_trial = float(np.sqrt(np.mean(r_trial * r_trial))) if r_trial.size else rms
|
||||
|
||||
if rms_trial < rms:
|
||||
theta = theta_trial
|
||||
lam *= 0.5
|
||||
else:
|
||||
lam *= 2.0
|
||||
|
||||
if np.linalg.norm(delta) < 1e-10:
|
||||
break
|
||||
if abs(rms - rms_trial) < 1e-12:
|
||||
break
|
||||
|
||||
return theta, history
|
||||
|
||||
|
||||
def pose_covariance(theta: np.ndarray,
|
||||
X_world: np.ndarray,
|
||||
obs_norm: np.ndarray,
|
||||
eps_jac: float = 1e-6) -> Tuple[np.ndarray, float, np.ndarray]:
|
||||
"""
|
||||
Returns:
|
||||
cov_theta_6x6, sigma2, residual_vector
|
||||
"""
|
||||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac, X_world, obs_norm)
|
||||
m = r.size
|
||||
n = theta.size
|
||||
dof = max(1, m - n)
|
||||
sigma2 = float((r @ r) / dof)
|
||||
|
||||
JTJ = J.T @ J
|
||||
cov = sigma2 * np.linalg.pinv(JTJ)
|
||||
return cov.astype(np.float64), sigma2, r
|
||||
|
||||
|
||||
def propagate_covariance(theta: np.ndarray,
|
||||
cov_theta: np.ndarray) -> Dict[str, Any]:
|
||||
"""
|
||||
Propagate pose covariance to camera center and Euler angle uncertainties.
|
||||
"""
|
||||
def camera_center_fn(th: np.ndarray) -> np.ndarray:
|
||||
_, _, c = theta_to_camera_pose(th)
|
||||
return c.astype(np.float64)
|
||||
|
||||
def euler_fn(th: np.ndarray) -> np.ndarray:
|
||||
R_wc, _, _ = theta_to_camera_pose(th)
|
||||
return np.array(R_to_euler_zyx(R_wc), dtype=np.float64) # deg
|
||||
|
||||
Jc, _ = numerical_jacobian(lambda th, *_: camera_center_fn(th), theta, 1e-6)
|
||||
cov_center = Jc @ cov_theta @ Jc.T
|
||||
|
||||
Je, _ = numerical_jacobian(lambda th, *_: euler_fn(th), theta, 1e-6)
|
||||
cov_euler = Je @ cov_theta @ Je.T
|
||||
|
||||
center_std_m = np.sqrt(np.maximum(0.0, np.diag(cov_center)))
|
||||
euler_std_deg = np.sqrt(np.maximum(0.0, np.diag(cov_euler)))
|
||||
|
||||
# Parameter std directly from covariance
|
||||
param_std = np.sqrt(np.maximum(0.0, np.diag(cov_theta)))
|
||||
rvec_std_deg = np.degrees(param_std[0:3])
|
||||
tvec_std_m = param_std[3:6]
|
||||
|
||||
return {
|
||||
"pose_covariance_6x6": cov_theta.tolist(),
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [float(x) for x in rvec_std_deg],
|
||||
"tvec_std_m": [float(x) for x in tvec_std_m],
|
||||
},
|
||||
"camera_center_std_m": [float(x) for x in center_std_m],
|
||||
"camera_center_std_mm": [float(x * 1000.0) for x in center_std_m],
|
||||
"orientation_std_deg": {
|
||||
"roll": float(euler_std_deg[0]),
|
||||
"pitch": float(euler_std_deg[1]),
|
||||
"yaw": float(euler_std_deg[2]),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Marker processing
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def build_object_corners_from_world_position(position_m: np.ndarray,
|
||||
marker_size_m: float) -> np.ndarray:
|
||||
"""
|
||||
Marker corners in world coordinates, assuming the marker frame is aligned
|
||||
with the world frame and only translated to 'position_m'.
|
||||
|
||||
This is the direct analogue of readTwoImages.py using marker center positions.
|
||||
"""
|
||||
h = marker_size_m / 2.0
|
||||
local = np.array([
|
||||
[-h, h, 0.0],
|
||||
[ h, h, 0.0],
|
||||
[ h, -h, 0.0],
|
||||
[-h, -h, 0.0],
|
||||
], dtype=np.float32)
|
||||
return local + position_m.reshape(1, 3)
|
||||
|
||||
|
||||
def solve_single_marker_pose(corners_px: np.ndarray,
|
||||
K: np.ndarray,
|
||||
D: np.ndarray,
|
||||
marker_size_m: float) -> Optional[Tuple[np.ndarray, np.ndarray]]:
|
||||
obj = marker_local_corners(marker_size_m)
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj,
|
||||
corners_px.astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE
|
||||
)
|
||||
if not success:
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj,
|
||||
corners_px.astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
flags=cv2.SOLVEPNP_ITERATIVE
|
||||
)
|
||||
if not success:
|
||||
return None
|
||||
return rvec.reshape(3), tvec.reshape(3)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Main
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Estimate camera pose from ArUco observation JSON")
|
||||
parser.add_argument("-i", "--input", required=True, help="*_aruco_detection.json")
|
||||
parser.add_argument("-robot", "--robot", required=True, help="robot.json with board markers")
|
||||
parser.add_argument("-outDir", "--outDir", default=None, help="Optional output directory")
|
||||
parser.add_argument("--minConfidence", type=float, default=0.0,
|
||||
help="Skip detections below this confidence")
|
||||
parser.add_argument("--minCommonMarkers", type=int, default=3,
|
||||
help="Minimum number of world markers required")
|
||||
parser.add_argument("--maxRmsPx", type=float, default=None,
|
||||
help="Optional soft warning threshold for final reprojection RMS in pixels")
|
||||
parser.add_argument("--epsJac", type=float, default=1e-6, help="Finite-difference epsilon")
|
||||
args = parser.parse_args()
|
||||
|
||||
detection_path = resolve_path(args.input)
|
||||
robot_path = resolve_path(args.robot)
|
||||
|
||||
detection = load_json(detection_path)
|
||||
marker_lookup = load_marker_lookup(robot_path)
|
||||
|
||||
K, D = load_intrinsics_from_detection(detection)
|
||||
|
||||
robot_marker_size = load_robot_marker_size(robot_path)
|
||||
det_marker_size = detection.get("vision_config", {}).get("MarkerSize", None)
|
||||
if det_marker_size is not None:
|
||||
marker_size_m = float(det_marker_size)
|
||||
elif robot_marker_size is not None:
|
||||
marker_size_m = float(robot_marker_size)
|
||||
else:
|
||||
marker_size_m = 0.025
|
||||
|
||||
detections = detection.get("detections", [])
|
||||
if not isinstance(detections, list):
|
||||
raise TypeError("detection['detections'] must be a list")
|
||||
|
||||
used_ids: List[int] = []
|
||||
used_world_positions: List[np.ndarray] = []
|
||||
used_obs_centers_px: List[np.ndarray] = []
|
||||
used_obs_centers_norm: List[np.ndarray] = []
|
||||
used_marker_cam_centers: List[np.ndarray] = []
|
||||
used_marker_meta: List[Dict[str, Any]] = []
|
||||
|
||||
sanity_notes: List[str] = []
|
||||
|
||||
for det in detections:
|
||||
if det.get("type", "aruco") != "aruco":
|
||||
continue
|
||||
|
||||
marker_id = int(det.get("marker_id", -1))
|
||||
if marker_id < 0:
|
||||
continue
|
||||
|
||||
if marker_id not in marker_lookup:
|
||||
continue
|
||||
|
||||
confidence = float(det.get("confidence", 1.0))
|
||||
if confidence < args.minConfidence:
|
||||
continue
|
||||
|
||||
corners = det.get("image_points_px", None)
|
||||
if corners is None:
|
||||
continue
|
||||
|
||||
corners_px = np.array(corners, dtype=np.float32).reshape(4, 2)
|
||||
center_from_corners = corners_px.mean(axis=0)
|
||||
|
||||
center_px = np.array(det.get("center_px", center_from_corners), dtype=np.float32).reshape(2)
|
||||
center_delta = float(np.linalg.norm(center_from_corners - center_px))
|
||||
if center_delta > 0.75:
|
||||
sanity_notes.append(
|
||||
f"marker {marker_id}: center_px differs from corner-mean by {center_delta:.2f}px"
|
||||
)
|
||||
|
||||
pnp = solve_single_marker_pose(corners_px, K, D, marker_size_m)
|
||||
if pnp is None:
|
||||
continue
|
||||
|
||||
rvec_m, tvec_m = pnp
|
||||
world_pos = marker_lookup[marker_id]["position"].astype(np.float32)
|
||||
|
||||
used_ids.append(marker_id)
|
||||
used_world_positions.append(world_pos)
|
||||
used_obs_centers_px.append(center_from_corners.astype(np.float32))
|
||||
used_obs_centers_norm.append(undistort_to_normalized(center_from_corners.reshape(1, 2), K, D)[0])
|
||||
used_marker_cam_centers.append(tvec_m.astype(np.float32))
|
||||
used_marker_meta.append({
|
||||
"marker_id": marker_id,
|
||||
"confidence": confidence,
|
||||
"center_px": [float(center_from_corners[0]), float(center_from_corners[1])],
|
||||
"marker_size_m": marker_size_m,
|
||||
})
|
||||
|
||||
# Unique / deduplicate by marker_id while preserving order
|
||||
dedup: Dict[int, int] = {}
|
||||
uniq_ids: List[int] = []
|
||||
uniq_world_positions: List[np.ndarray] = []
|
||||
uniq_obs_px: List[np.ndarray] = []
|
||||
uniq_obs_norm: List[np.ndarray] = []
|
||||
uniq_cam_centers: List[np.ndarray] = []
|
||||
uniq_meta: List[Dict[str, Any]] = []
|
||||
|
||||
for idx, mid in enumerate(used_ids):
|
||||
if mid in dedup:
|
||||
continue
|
||||
dedup[mid] = idx
|
||||
uniq_ids.append(mid)
|
||||
uniq_world_positions.append(used_world_positions[idx])
|
||||
uniq_obs_px.append(used_obs_centers_px[idx])
|
||||
uniq_obs_norm.append(used_obs_centers_norm[idx])
|
||||
uniq_cam_centers.append(used_marker_cam_centers[idx])
|
||||
uniq_meta.append(used_marker_meta[idx])
|
||||
|
||||
if len(uniq_ids) < args.minCommonMarkers:
|
||||
raise RuntimeError(
|
||||
f"Need at least {args.minCommonMarkers} common markers; found {len(uniq_ids)}: {uniq_ids}"
|
||||
)
|
||||
|
||||
X_world = np.stack(uniq_world_positions, axis=0).astype(np.float64)
|
||||
obs_px = np.stack(uniq_obs_px, axis=0).astype(np.float64)
|
||||
obs_norm = np.stack(uniq_obs_norm, axis=0).astype(np.float64)
|
||||
marker_cam_centers = np.stack(uniq_cam_centers, axis=0).astype(np.float64)
|
||||
|
||||
# Initial pose from rigid transform of per-marker camera-frame centers to world positions
|
||||
# B ≈ R A + t -> world = R * camera + t
|
||||
R_cw_init, t_cw_init = rigid_transform_no_scale(marker_cam_centers, X_world)
|
||||
R_wc_init = R_cw_init.T
|
||||
t_wc_init = (-R_wc_init @ t_cw_init).reshape(3)
|
||||
|
||||
omega_init = cv2.Rodrigues(R_wc_init)[0].reshape(3)
|
||||
theta0 = pack_params(omega_init, t_wc_init)
|
||||
|
||||
theta_opt, hist = lm_solve(
|
||||
theta0=theta0,
|
||||
X_world=X_world,
|
||||
obs_norm=obs_norm,
|
||||
max_iter=60,
|
||||
eps_jac=args.epsJac,
|
||||
lambda_init=1e-3,
|
||||
)
|
||||
|
||||
R_wc, t_wc, camera_center_world = theta_to_camera_pose(theta_opt)
|
||||
|
||||
cov_theta, sigma2, residual_vec = pose_covariance(
|
||||
theta_opt, X_world, obs_norm, eps_jac=args.epsJac
|
||||
)
|
||||
propagated = propagate_covariance(theta_opt, cov_theta)
|
||||
|
||||
# Exact pixel-space reprojection statistics
|
||||
proj_pts, _ = cv2.projectPoints(
|
||||
X_world.reshape(-1, 1, 3).astype(np.float32),
|
||||
theta_opt[0:3].reshape(3, 1).astype(np.float32),
|
||||
theta_opt[3:6].reshape(3, 1).astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
)
|
||||
proj_pts = proj_pts.reshape(-1, 2)
|
||||
reproj_err_px = np.linalg.norm(proj_pts - obs_px, axis=1)
|
||||
rms_px = float(np.sqrt(np.mean(reproj_err_px ** 2))) if reproj_err_px.size else 0.0
|
||||
median_px = float(np.median(reproj_err_px)) if reproj_err_px.size else 0.0
|
||||
max_px = float(np.max(reproj_err_px)) if reproj_err_px.size else 0.0
|
||||
|
||||
if args.maxRmsPx is not None and rms_px > args.maxRmsPx:
|
||||
print(f"[WARN] Final reprojection RMS is {rms_px:.3f}px (threshold {args.maxRmsPx:.3f}px).")
|
||||
|
||||
# Convert outputs
|
||||
roll, pitch, yaw = R_to_euler_zyx(R_wc)
|
||||
position_mm = (camera_center_world * 1000.0).astype(float).tolist()
|
||||
|
||||
# Reproject each used marker center for QA
|
||||
per_marker_results = []
|
||||
proj_pts_exact, _ = cv2.projectPoints(
|
||||
X_world.reshape(-1, 1, 3).astype(np.float32),
|
||||
theta_opt[0:3].reshape(3, 1).astype(np.float32),
|
||||
theta_opt[3:6].reshape(3, 1).astype(np.float32),
|
||||
K,
|
||||
D,
|
||||
)
|
||||
proj_pts_exact = proj_pts_exact.reshape(-1, 2)
|
||||
|
||||
for idx, mid in enumerate(uniq_ids):
|
||||
x = proj_pts_exact[idx]
|
||||
err = float(np.linalg.norm(x - obs_px[idx]))
|
||||
per_marker_results.append({
|
||||
"marker_id": int(mid),
|
||||
"observed_center_px": [float(obs_px[idx, 0]), float(obs_px[idx, 1])],
|
||||
"projected_center_px": [float(x[0]), float(x[1])],
|
||||
"reprojection_error_px": err,
|
||||
"confidence": float(uniq_meta[idx]["confidence"]),
|
||||
})
|
||||
|
||||
# Output directory
|
||||
in_base = os.path.splitext(os.path.basename(detection_path))[0]
|
||||
out_name = in_base.replace("_aruco_detection", "_camera_pose") + ".json"
|
||||
|
||||
if args.outDir is not None:
|
||||
out_dir = resolve_path(args.outDir)
|
||||
else:
|
||||
out_dir = os.path.dirname(detection_path) or "."
|
||||
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
out_json = os.path.join(out_dir, out_name)
|
||||
|
||||
output = {
|
||||
"schema_version": "1.0",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"source": {
|
||||
"detection_json": detection_path,
|
||||
"robot_json": robot_path,
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": detection.get("camera", {}).get("camera_id", "unknown"),
|
||||
"camera_matrix": K.tolist(),
|
||||
"distortion_coefficients": D.reshape(-1).tolist(),
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": float(marker_size_m),
|
||||
"num_used_markers": int(len(uniq_ids)),
|
||||
"used_marker_ids": [int(x) for x in uniq_ids],
|
||||
"history": hist,
|
||||
"residual_rms_px": float(rms_px),
|
||||
"residual_median_px": float(median_px),
|
||||
"residual_max_px": float(max_px),
|
||||
"sigma2_normalized": float(sigma2),
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": R_wc.tolist(),
|
||||
"translation_m": [float(x) for x in t_wc.tolist()],
|
||||
"rvec_rad": [float(x) for x in theta_opt[0:3].tolist()],
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [float(x) for x in camera_center_world.tolist()],
|
||||
"position_mm": [float(x) for x in position_mm],
|
||||
"orientation_deg": {
|
||||
"roll": float(roll),
|
||||
"pitch": float(pitch),
|
||||
"yaw": float(yaw),
|
||||
},
|
||||
},
|
||||
"uncertainty": propagated,
|
||||
},
|
||||
"observations": {
|
||||
"markers": per_marker_results,
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": sanity_notes,
|
||||
},
|
||||
}
|
||||
|
||||
save_json(out_json, output)
|
||||
print(f"[INFO] Saved camera pose JSON: {out_json}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except Exception as exc:
|
||||
print(f"[ERROR] {exc}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
685
pipeline/2_estimate_camera_pose_from_aruco_json.py
Normal file
685
pipeline/2_estimate_camera_pose_from_aruco_json.py
Normal file
@@ -0,0 +1,685 @@
|
||||
#!/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 = {}
|
||||
|
||||
|
||||
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
|
||||
),
|
||||
"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()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,72 +0,0 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-28T21:28:35.973154Z",
|
||||
"source_robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json",
|
||||
"source_detections": [
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json",
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json",
|
||||
"C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json"
|
||||
],
|
||||
"solver": {
|
||||
"final_cost": 26976.87543820547,
|
||||
"status": 0,
|
||||
"message": "The maximum number of function evaluations is exceeded."
|
||||
},
|
||||
"robot_pose": {
|
||||
"state": {
|
||||
"x": 100.21525656040845,
|
||||
"y": 29.397626553503173,
|
||||
"z": -36.04825291833897,
|
||||
"a": -120.21337951359496,
|
||||
"b": 22.00000000000008,
|
||||
"c": 90.99999999999989,
|
||||
"e": 10.000000000000258
|
||||
},
|
||||
"uncertainty": {
|
||||
"x_mm": 17194.235819424397,
|
||||
"y_mm": 6014.1408132596725,
|
||||
"z_mm": 3621.298501957444,
|
||||
"a_deg": 8796.009134059024,
|
||||
"b_deg": 12562.017200522667,
|
||||
"c_deg": 12562.017199380893,
|
||||
"e_mm": 125620.17196327279
|
||||
},
|
||||
"confidence": {
|
||||
"x": 0.0,
|
||||
"y": 6.444409678452354e-262,
|
||||
"z": 5.358019964976179e-158,
|
||||
"a": 0.0,
|
||||
"b": 0.0,
|
||||
"c": 0.0,
|
||||
"e": 0.36787944117144233
|
||||
}
|
||||
},
|
||||
"statistics": {
|
||||
"observation_count": 34,
|
||||
"camera_count": 3,
|
||||
"marker_count": 23,
|
||||
"observed_marker_count": 18,
|
||||
"mean_detector_confidence": 0.5871913728875101,
|
||||
"mean_weighted_confidence": 0.5774319023193206,
|
||||
"mean_reprojection_error_px": 236.06726174073253,
|
||||
"quality_means": {
|
||||
"size": 0.8143773112577551,
|
||||
"aspect": 0.834768084426124,
|
||||
"border": 0.9237745098039215,
|
||||
"homography": 0.7934741744632792
|
||||
},
|
||||
"quality_config": {
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.01,
|
||||
"aspect_factor": 0.01,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.01,
|
||||
"homography_factor": 0.01
|
||||
}
|
||||
}
|
||||
}
|
||||
921
pipeline/readTwoImages.py
Normal file
921
pipeline/readTwoImages.py
Normal file
@@ -0,0 +1,921 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
readCamPositionTwo.py
|
||||
|
||||
Two-camera ArUco detection with joint optimization of both camera extrinsics
|
||||
against known machine-frame reference markers, plus triangulation of unknown
|
||||
marker positions. Outputs camera pose and marker poses in machine coordinates,
|
||||
with CSV and JSON similar to the single-camera script.
|
||||
|
||||
Dependencies: numpy, opencv-python (cv2)
|
||||
Optional but NOT required: SciPy (we implement a simple Levenberg–Marquardt).
|
||||
|
||||
Usage example:
|
||||
python3 readTwoImages.py -i snapshot_video0_1764531874081.jpg -i snapshot_video1_1764531874081.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||||
python3 readTwoImages.py -i snapshot_video0_1764524369655.jpg -i snapshot_video1_1764524369655.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||||
|
||||
python3 readTwoImages.py -i snapshot_video0_1765009029764.jpg -i snapshot_video1_1765009029764.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||||
|
||||
NEW:
|
||||
python3 readTwoImages.py -i img0.jpg -i img1.jpg -npz cam0.npz -npz cam1.npz -settings settings.json -outDir results/
|
||||
|
||||
Settings JSON is expected to contain:
|
||||
{
|
||||
"coordinateSystem": {
|
||||
"KnownMarkers": {
|
||||
"50": [0.0, 0.0, 0.0],
|
||||
"71": [0.140, 0.0, 0.0],
|
||||
"101": [0.0, -0.080, 0.0]
|
||||
}
|
||||
}
|
||||
}
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import json
|
||||
import time
|
||||
from typing import Dict, Tuple, List
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
# ---------------- Configuration defaults ----------------
|
||||
AXIS_LENGTH_M = 0.05
|
||||
|
||||
# ---------------- Utilities ----------------
|
||||
def load_intrinsics_npz(npz_path: str) -> Tuple[np.ndarray, np.ndarray]:
|
||||
print("NPZ reading of file:", npz_path)
|
||||
if os.path.exists(npz_path):
|
||||
data = np.load(npz_path)
|
||||
for k in ('camera_matrix', 'mtx', 'K'):
|
||||
if k in data:
|
||||
camera_matrix = data[k].astype(np.float32)
|
||||
break
|
||||
else:
|
||||
raise KeyError("Camera matrix not found in NPZ.")
|
||||
for k in ('dist_coeffs', 'dist', 'D'):
|
||||
if k in data:
|
||||
dist = data[k].astype(np.float32).reshape(-1,1)
|
||||
break
|
||||
else:
|
||||
dist = np.zeros((5,1), dtype=np.float32)
|
||||
print("NPZ loaded:", npz_path)
|
||||
return camera_matrix, dist
|
||||
# Fallback default intrinsics
|
||||
camera_matrix = np.array([[1400, 0, 640],
|
||||
[0, 1400, 360],
|
||||
[0, 0, 1]], dtype=np.float32)
|
||||
dist_coeffs = np.zeros((5,1), dtype=np.float32)
|
||||
print("[WARN] Using default approximate intrinsics.")
|
||||
return camera_matrix, dist_coeffs
|
||||
|
||||
|
||||
def get_aruco_detector(dict_name: str):
|
||||
mapping = {
|
||||
'DICT_4X4_250': cv2.aruco.DICT_4X4_250,
|
||||
'DICT_5X5_100': cv2.aruco.DICT_5X5_100,
|
||||
'DICT_6X6_250': cv2.aruco.DICT_6X6_250,
|
||||
'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL,
|
||||
}
|
||||
if dict_name not in mapping:
|
||||
dict_id = cv2.aruco.DICT_4X4_250
|
||||
else:
|
||||
dict_id = mapping[dict_name]
|
||||
dictionary = cv2.aruco.getPredefinedDictionary(dict_id)
|
||||
try:
|
||||
params = cv2.aruco.DetectorParameters()
|
||||
except Exception:
|
||||
params = cv2.aruco.DetectorParameters_create()
|
||||
try:
|
||||
detector = cv2.aruco.ArucoDetector(dictionary, params)
|
||||
return detector, None
|
||||
except Exception:
|
||||
return None, (dictionary, params)
|
||||
|
||||
|
||||
def detect_markers(image: np.ndarray, detector_tuple):
|
||||
detector, fallback = detector_tuple
|
||||
if detector is not None:
|
||||
corners, ids, rejected = detector.detectMarkers(image)
|
||||
else:
|
||||
dictionary, params = fallback
|
||||
corners, ids, rejected = cv2.aruco.detectMarkers(image, dictionary, parameters=params)
|
||||
return corners, ids, rejected
|
||||
|
||||
|
||||
def corners_to_image_points(corners: np.ndarray) -> np.ndarray:
|
||||
return corners.reshape(4,2).astype(np.float32)
|
||||
|
||||
|
||||
def marker_center_from_corners(corners: np.ndarray) -> np.ndarray:
|
||||
pts = corners.reshape(4,2)
|
||||
return pts.mean(axis=0).astype(np.float32)
|
||||
|
||||
|
||||
def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
return R
|
||||
|
||||
|
||||
def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""Find R,t s.t. B ≈ R A + t. A,B: Nx3."""
|
||||
assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3"
|
||||
N = A.shape[0]
|
||||
if N < 2:
|
||||
raise ValueError("Need at least 2 points; 3+ recommended.")
|
||||
centroid_A = A.mean(axis=0)
|
||||
centroid_B = B.mean(axis=0)
|
||||
AA = A - centroid_A
|
||||
BB = B - centroid_B
|
||||
H = AA.T @ BB
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
if np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1
|
||||
R = Vt.T @ U.T
|
||||
t = centroid_B - R @ centroid_A
|
||||
return R.astype(np.float32), t.astype(np.float32)
|
||||
|
||||
|
||||
def undistort_to_normalized(points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray:
|
||||
"""points_px: Nx2 pixel. Returns Nx2 normalized coords (x,y) where projection is x=Xp/Z, y=Yp/Z.
|
||||
cv2.undistortPoints with P=None yields normalized coordinates.
|
||||
"""
|
||||
pts = points_px.reshape(-1,1,2).astype(np.float32)
|
||||
und = cv2.undistortPoints(pts, K, D, P=None) # returns Nx1x2
|
||||
return und.reshape(-1,2)
|
||||
|
||||
|
||||
# ---------------- Joint optimization (LM, numerical Jacobian) ----------------
|
||||
|
||||
def pack_params(omega1, t1, omega2, t2) -> np.ndarray:
|
||||
return np.hstack([omega1, t1, omega2, t2]).astype(np.float64)
|
||||
|
||||
|
||||
def unpack_params(theta: np.ndarray):
|
||||
omega1 = theta[0:3]
|
||||
t1 = theta[3:6]
|
||||
omega2 = theta[6:9]
|
||||
t2 = theta[9:12]
|
||||
return omega1, t1, omega2, t2
|
||||
|
||||
|
||||
def residuals_centers_normalized(theta: np.ndarray,
|
||||
X_mach: np.ndarray,
|
||||
obs1_norm: np.ndarray,
|
||||
obs2_norm: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Compute residuals in normalized coordinates (no intrinsics, no distortion).
|
||||
For camera j: X_cam = R_j X_mach + t_j; proj: (x/z, y/z).
|
||||
Returns stacked residuals [r1; r2] shape (4N,), where N = number of references.
|
||||
"""
|
||||
omega1, t1, omega2, t2 = unpack_params(theta)
|
||||
R1 = cv2.Rodrigues(omega1)[0]
|
||||
R2 = cv2.Rodrigues(omega2)[0]
|
||||
# Camera 1 projections
|
||||
X_cam1 = (R1 @ X_mach.T + t1.reshape(3,1)).T # Nx3
|
||||
uv1 = X_cam1[:, :2] / X_cam1[:, 2:3]
|
||||
r1 = (obs1_norm - uv1).reshape(-1)
|
||||
# Camera 2 projections
|
||||
X_cam2 = (R2 @ X_mach.T + t2.reshape(3,1)).T
|
||||
uv2 = X_cam2[:, :2] / X_cam2[:, 2:3]
|
||||
r2 = (obs2_norm - uv2).reshape(-1)
|
||||
return np.hstack([r1, r2])
|
||||
|
||||
|
||||
def numerical_jacobian(f, theta: np.ndarray, eps: float, *args) -> np.ndarray:
|
||||
"""Finite-difference Jacobian: forward differences."""
|
||||
r0 = f(theta, *args)
|
||||
m = r0.size
|
||||
n = theta.size
|
||||
J = np.zeros((m, n), dtype=np.float64)
|
||||
for k in range(n):
|
||||
th = theta.copy()
|
||||
th[k] += eps
|
||||
rk = f(th, *args)
|
||||
J[:, k] = (rk - r0) / eps
|
||||
return J, r0
|
||||
|
||||
|
||||
def lm_solve(theta0: np.ndarray,
|
||||
X_mach: np.ndarray,
|
||||
obs1_norm: np.ndarray,
|
||||
obs2_norm: np.ndarray,
|
||||
max_iter: int = 50,
|
||||
eps_jac: float = 1e-6,
|
||||
lambda_init: float = 1e-3) -> Tuple[np.ndarray, Dict]:
|
||||
"""Simple Levenberg–Marquardt on center normalized residuals."""
|
||||
lam = lambda_init
|
||||
theta = theta0.copy()
|
||||
history = {"iters": [], "rms": []}
|
||||
for it in range(max_iter):
|
||||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac,
|
||||
X_mach, obs1_norm, obs2_norm)
|
||||
rms = float(np.sqrt(np.mean(r*r))) if r.size else 0.0
|
||||
history["iters"].append(it)
|
||||
history["rms"].append(rms)
|
||||
# Normal equations with damping
|
||||
JTJ = J.T @ J
|
||||
g = J.T @ r
|
||||
H = JTJ + lam * np.eye(JTJ.shape[0])
|
||||
try:
|
||||
delta = -np.linalg.solve(H, g)
|
||||
except np.linalg.LinAlgError:
|
||||
# Fallback to least squares
|
||||
delta, *_ = np.linalg.lstsq(H, -g, rcond=None)
|
||||
theta_trial = theta + delta
|
||||
r_trial = residuals_centers_normalized(theta_trial, X_mach, obs1_norm, obs2_norm)
|
||||
rms_trial = float(np.sqrt(np.mean(r_trial*r_trial)))
|
||||
if rms_trial < rms: # improve
|
||||
theta = theta_trial
|
||||
lam *= 0.5
|
||||
else:
|
||||
lam *= 2.0
|
||||
# Convergence criteria
|
||||
if np.linalg.norm(delta) < 1e-9 or abs(rms - rms_trial) < 1e-9:
|
||||
break
|
||||
return theta, history
|
||||
|
||||
|
||||
# ---------------- Triangulation ----------------
|
||||
|
||||
def build_projection_matrix(K: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
return K @ np.hstack([R, t.reshape(3,1)])
|
||||
|
||||
|
||||
def triangulate_center(P1: np.ndarray, P2: np.ndarray, u1: np.ndarray, u2: np.ndarray) -> np.ndarray:
|
||||
# u1,u2: (2,) pixel coordinates
|
||||
x1 = u1.reshape(2,1)
|
||||
x2 = u2.reshape(2,1)
|
||||
X_h = cv2.triangulatePoints(P1, P2, x1, x2)
|
||||
X = (X_h[:3] / X_h[3]).reshape(3)
|
||||
return X.astype(np.float32)
|
||||
|
||||
|
||||
# ---------------- Main ----------------
|
||||
def main():
|
||||
print("Started")
|
||||
|
||||
parser = argparse.ArgumentParser(description="Two-camera ArUco joint optimization and triangulation")
|
||||
|
||||
parser.add_argument('-i', '--images', action='append', required=True,
|
||||
help="Two image paths: first camera then second camera")
|
||||
|
||||
parser.add_argument('-npz', '--npz', action='append', required=True,
|
||||
help="Two NPZ intrinsics paths: cam1 then cam2")
|
||||
|
||||
parser.add_argument('-markerSizeMM', '--markerSizeMM', type=float, default=25.0,
|
||||
help="Marker side length in millimeters")
|
||||
|
||||
parser.add_argument('--dict', default='DICT_4X4_250',
|
||||
help="ArUco dictionary name")
|
||||
|
||||
parser.add_argument('-settings', type=str, default=None,
|
||||
help="Json settings file containing machine KnownMarkers")
|
||||
|
||||
# ============================================================
|
||||
# NEW OPTIONAL PARAMETER (100% backward compatible)
|
||||
# ============================================================
|
||||
parser.add_argument('-outDir', '--outDir',
|
||||
type=str,
|
||||
default=None,
|
||||
help="Optional output directory")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if len(args.images) != 2 or len(args.npz) != 2:
|
||||
print("[ERROR] Provide exactly two images and two intrinsics NPZ files.")
|
||||
sys.exit(1)
|
||||
|
||||
img1 = cv2.imread(args.images[0])
|
||||
img2 = cv2.imread(args.images[1])
|
||||
|
||||
draw1 = img1.copy()
|
||||
draw2 = img2.copy()
|
||||
|
||||
h, w = draw1.shape[:2]
|
||||
|
||||
drawPNG1 = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
drawPNG2 = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
|
||||
if img1 is None or img2 is None:
|
||||
print("[ERROR] Cannot read one of the images.")
|
||||
sys.exit(1)
|
||||
|
||||
K1, D1 = load_intrinsics_npz(args.npz[0])
|
||||
K2, D2 = load_intrinsics_npz(args.npz[1])
|
||||
|
||||
# Marker 3D local geometry (square corners)
|
||||
half = (args.markerSizeMM / 1000.0) / 2.0
|
||||
|
||||
obj_points = np.array([
|
||||
[-half, half, 0.0],
|
||||
[ half, half, 0.0],
|
||||
[ half, -half, 0.0],
|
||||
[-half, -half, 0.0],
|
||||
], dtype=np.float32)
|
||||
|
||||
# Read settings for machine known markers
|
||||
known_markers: Dict[int, np.ndarray] = {}
|
||||
|
||||
if args.settings is not None and os.path.exists(args.settings):
|
||||
|
||||
with open(args.settings, 'r') as f:
|
||||
settings = json.load(f)
|
||||
|
||||
for marker_id, coords in settings['coordinateSystem']['KnownMarkers'].items():
|
||||
known_markers[int(marker_id)] = np.array(coords, dtype=np.float32)
|
||||
|
||||
print("[INFO] Loaded KnownMarkers from settings.")
|
||||
|
||||
else:
|
||||
|
||||
known_markers = {
|
||||
50: np.array([0.0, 0.0, 0.0], dtype=np.float32),
|
||||
71: np.array([0.140, 0.0, 0.0], dtype=np.float32),
|
||||
101: np.array([0.0, -0.080, 0.0], dtype=np.float32),
|
||||
}
|
||||
|
||||
print("[WARN] Using default KnownMarkers; provide -settings for your machine.")
|
||||
|
||||
# Detect markers in both images
|
||||
detector_tuple = get_aruco_detector(args.dict)
|
||||
|
||||
corners1, ids1, _ = detect_markers(img1, detector_tuple)
|
||||
corners2, ids2, _ = detect_markers(img2, detector_tuple)
|
||||
|
||||
if ids1 is None or ids2 is None:
|
||||
print("[ERROR] No markers detected in one or both images.")
|
||||
sys.exit(1)
|
||||
|
||||
ids1 = ids1.flatten().tolist()
|
||||
ids2 = ids2.flatten().tolist()
|
||||
|
||||
# Neu: merken, welche Kamera welchen Marker gesehen hat
|
||||
seen_by = {}
|
||||
|
||||
for mid in ids1:
|
||||
seen_by[mid] = seen_by.get(mid, 0) | 1
|
||||
|
||||
for mid in ids2:
|
||||
seen_by[mid] = seen_by.get(mid, 0) | 2
|
||||
|
||||
# Build dicts
|
||||
def build_marker_dict(img, corners_list, ids, K, D, draw=False):
|
||||
|
||||
centers = {}
|
||||
corners_dict = {}
|
||||
poses = {}
|
||||
|
||||
for i, mid in enumerate(ids):
|
||||
|
||||
C = corners_list[i]
|
||||
|
||||
corners_dict[int(mid)] = C
|
||||
centers[int(mid)] = marker_center_from_corners(C)
|
||||
|
||||
# Pose from single marker
|
||||
img_pts = corners_to_image_points(C)
|
||||
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj_points,
|
||||
img_pts,
|
||||
K,
|
||||
D,
|
||||
flags=cv2.SOLVEPNP_IPPE_SQUARE
|
||||
)
|
||||
|
||||
if not success:
|
||||
success, rvec, tvec = cv2.solvePnP(
|
||||
obj_points,
|
||||
img_pts,
|
||||
K,
|
||||
D
|
||||
)
|
||||
|
||||
if success:
|
||||
|
||||
poses[int(mid)] = (
|
||||
rvec.reshape(3,1),
|
||||
tvec.reshape(3,1)
|
||||
)
|
||||
|
||||
if draw:
|
||||
cv2.drawFrameAxes(draw1, K, D, rvec, tvec, 0.02)
|
||||
cv2.drawFrameAxes(drawPNG1, K, D, rvec, tvec, 0.02)
|
||||
|
||||
return centers, corners_dict, poses
|
||||
|
||||
centers1, corners_dict1, poses1 = build_marker_dict(
|
||||
img1, corners1, ids1, K1, D1, draw=True
|
||||
)
|
||||
|
||||
centers2, corners_dict2, poses2 = build_marker_dict(
|
||||
img2, corners2, ids2, K2, D2
|
||||
)
|
||||
|
||||
# Common reference markers
|
||||
common_refs = [
|
||||
mid for mid in known_markers.keys()
|
||||
if (mid in centers1 and mid in centers2)
|
||||
]
|
||||
|
||||
if len(common_refs) < 3:
|
||||
print(f"[ERROR] Need ≥3 common reference markers in both cameras; found {len(common_refs)}: {common_refs}")
|
||||
sys.exit(1)
|
||||
|
||||
# Initial extrinsics
|
||||
def init_extrinsics_from_rigid(poses_cam, refs):
|
||||
|
||||
A = []
|
||||
B = []
|
||||
|
||||
for mid in refs:
|
||||
if mid in poses_cam:
|
||||
_, tvec = poses_cam[mid]
|
||||
A.append(tvec.flatten())
|
||||
B.append(known_markers[mid])
|
||||
|
||||
if len(A) < 2:
|
||||
raise RuntimeError("Insufficient reference poses for rigid transform init.")
|
||||
|
||||
A = np.stack(A, axis=0)
|
||||
B = np.stack(B, axis=0)
|
||||
|
||||
R_cm, t_cm = rigid_transform_no_scale(A, B)
|
||||
|
||||
R_mc = R_cm.T
|
||||
t_mc = - R_mc @ t_cm
|
||||
|
||||
return R_mc.astype(np.float32), t_mc.astype(np.float32)
|
||||
|
||||
R1_init, t1_init = init_extrinsics_from_rigid(poses1, common_refs)
|
||||
R2_init, t2_init = init_extrinsics_from_rigid(poses2, common_refs)
|
||||
|
||||
# Observations
|
||||
X_mach_refs = np.stack([
|
||||
known_markers[mid] for mid in common_refs
|
||||
], axis=0).astype(np.float32)
|
||||
|
||||
obs1_px = np.stack([
|
||||
centers1[mid] for mid in common_refs
|
||||
], axis=0).astype(np.float32)
|
||||
|
||||
obs2_px = np.stack([
|
||||
centers2[mid] for mid in common_refs
|
||||
], axis=0).astype(np.float32)
|
||||
|
||||
obs1_norm = undistort_to_normalized(obs1_px, K1, D1)
|
||||
obs2_norm = undistort_to_normalized(obs2_px, K2, D2)
|
||||
|
||||
omega1_init = cv2.Rodrigues(R1_init)[0].reshape(3)
|
||||
omega2_init = cv2.Rodrigues(R2_init)[0].reshape(3)
|
||||
|
||||
theta0 = pack_params(
|
||||
omega1_init,
|
||||
t1_init.reshape(3),
|
||||
omega2_init,
|
||||
t2_init.reshape(3)
|
||||
)
|
||||
|
||||
theta_opt, hist = lm_solve(
|
||||
theta0,
|
||||
X_mach_refs,
|
||||
obs1_norm,
|
||||
obs2_norm,
|
||||
max_iter=60,
|
||||
eps_jac=1e-6,
|
||||
lambda_init=1e-3
|
||||
)
|
||||
|
||||
omega1, t1, omega2, t2 = unpack_params(theta_opt)
|
||||
|
||||
R1_opt = cv2.Rodrigues(omega1)[0]
|
||||
R2_opt = cv2.Rodrigues(omega2)[0]
|
||||
|
||||
# Camera pose in machine coordinates
|
||||
R1_cm = R1_opt.T
|
||||
t1_cm = - R1_cm @ t1
|
||||
|
||||
R2_cm = R2_opt.T
|
||||
t2_cm = - R2_cm @ t2
|
||||
|
||||
# Projection matrices
|
||||
P1 = build_projection_matrix(K1, R1_opt, t1)
|
||||
P2 = build_projection_matrix(K2, R2_opt, t2)
|
||||
|
||||
# Collect markers
|
||||
all_ids = set(ids1) | set(ids2)
|
||||
|
||||
rows = [("id", "x_mm", "y_mm", "z_mm", "roll_deg", "pitch_deg", "yaw_deg", "seen_by")]
|
||||
|
||||
marker_list = []
|
||||
|
||||
def R_to_euler_zyx(R: np.ndarray):
|
||||
|
||||
yaw = float(np.degrees(np.arctan2(R[1,0], R[0,0])))
|
||||
|
||||
sp = np.sqrt(R[2,1]**2 + R[2,2]**2)
|
||||
|
||||
pitch = float(np.degrees(np.arctan2(-R[2,0], sp)))
|
||||
|
||||
roll = float(np.degrees(np.arctan2(R[2,1], R[2,2])))
|
||||
|
||||
return roll, pitch, yaw
|
||||
|
||||
cam1_roll, cam1_pitch, cam1_yaw = R_to_euler_zyx(R1_cm)
|
||||
cam2_roll, cam2_pitch, cam2_yaw = R_to_euler_zyx(R2_cm)
|
||||
|
||||
c1_mm = (t1_cm * 1000.0).tolist()
|
||||
|
||||
rows.append((
|
||||
"camera 0",
|
||||
f"{c1_mm[0]:.2f}",
|
||||
f"{c1_mm[1]:.2f}",
|
||||
f"{c1_mm[2]:.2f}",
|
||||
f"{cam1_roll:.3f}",
|
||||
f"{cam1_pitch:.3f}",
|
||||
f"{cam1_yaw:.3f}"
|
||||
))
|
||||
|
||||
c2_mm = (t2_cm * 1000.0).tolist()
|
||||
|
||||
rows.append((
|
||||
"camera 1",
|
||||
f"{c2_mm[0]:.2f}",
|
||||
f"{c2_mm[1]:.2f}",
|
||||
f"{c2_mm[2]:.2f}",
|
||||
f"{cam2_roll:.3f}",
|
||||
f"{cam2_pitch:.3f}",
|
||||
f"{cam2_yaw:.3f}"
|
||||
))
|
||||
|
||||
# Triangulate and output markers
|
||||
def orientation_in_machine(mid: int):
|
||||
|
||||
if mid in poses1:
|
||||
Rm_cam = rvec_to_R(poses1[mid][0])
|
||||
Rm_machine = R1_cm @ Rm_cam
|
||||
|
||||
elif mid in poses2:
|
||||
Rm_cam = rvec_to_R(poses2[mid][0])
|
||||
Rm_machine = R2_cm @ Rm_cam
|
||||
|
||||
else:
|
||||
Rm_machine = np.eye(3, dtype=np.float32)
|
||||
|
||||
return R_to_euler_zyx(Rm_machine)
|
||||
|
||||
for mid in sorted(all_ids):
|
||||
|
||||
if mid in centers1 and mid in centers2:
|
||||
|
||||
X_mach = triangulate_center(
|
||||
P1,
|
||||
P2,
|
||||
centers1[mid],
|
||||
centers2[mid]
|
||||
)
|
||||
|
||||
elif mid in poses1:
|
||||
|
||||
X_mach = (
|
||||
R1_cm @ poses1[mid][1].flatten() + t1_cm
|
||||
)
|
||||
|
||||
elif mid in poses2:
|
||||
|
||||
X_mach = (
|
||||
R2_cm @ poses2[mid][1].flatten() + t2_cm
|
||||
)
|
||||
|
||||
else:
|
||||
continue
|
||||
|
||||
roll, pitch, yaw = orientation_in_machine(mid)
|
||||
|
||||
x_mm, y_mm, z_mm = (X_mach * 1000.0).tolist()
|
||||
|
||||
rows.append((
|
||||
mid,
|
||||
f"{x_mm:.2f}",
|
||||
f"{y_mm:.2f}",
|
||||
f"{z_mm:.2f}",
|
||||
f"{roll:.3f}",
|
||||
f"{pitch:.3f}",
|
||||
f"{yaw:.3f}",
|
||||
seen_by.get(mid, 0)
|
||||
))
|
||||
|
||||
marker_list.append({
|
||||
"id": int(mid),
|
||||
"position_mm": [float(x_mm), float(y_mm), float(z_mm)],
|
||||
"orientation_deg": {
|
||||
"roll": float(roll),
|
||||
"pitch": float(pitch),
|
||||
"yaw": float(yaw)
|
||||
}
|
||||
})
|
||||
|
||||
# ============================================================
|
||||
# OUTPUT DIRECTORY HANDLING
|
||||
# ============================================================
|
||||
|
||||
base1 = os.path.splitext(os.path.basename(args.images[0]))[0]
|
||||
base2 = os.path.splitext(os.path.basename(args.images[1]))[0]
|
||||
|
||||
if args.outDir is not None:
|
||||
|
||||
out_dir = args.outDir
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
else:
|
||||
|
||||
out_dir = os.path.dirname(args.images[0])
|
||||
|
||||
if out_dir == "":
|
||||
out_dir = "."
|
||||
|
||||
# Save CSV & JSON
|
||||
out_csv = os.path.join(out_dir, f"{base1}_two_cam.csv")
|
||||
out_json = os.path.join(out_dir, f"{base1}_two_cam.json")
|
||||
|
||||
try:
|
||||
import csv
|
||||
|
||||
with open(out_csv, 'w', newline='') as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerows(rows)
|
||||
|
||||
print(f"[INFO] Saved CSV poses to '{out_csv}'.")
|
||||
|
||||
except Exception as e:
|
||||
print(f"[WARN] Could not save CSV: {e}")
|
||||
|
||||
json_data = {
|
||||
"metadata": {
|
||||
"timestamp": time.strftime("%Y-%m-%d %H:%M:%S"),
|
||||
"reference_markers": common_refs,
|
||||
"dict": args.dict,
|
||||
"marker_size_mm": args.markerSizeMM,
|
||||
"description": "Two-camera joint optimization with triangulation"
|
||||
},
|
||||
"cameras": [
|
||||
{
|
||||
"id": "camera1",
|
||||
"position_mm": [float(x) for x in (t1_cm * 1000.0)],
|
||||
"orientation_deg": {
|
||||
"roll": cam1_roll,
|
||||
"pitch": cam1_pitch,
|
||||
"yaw": cam1_yaw
|
||||
},
|
||||
},
|
||||
{
|
||||
"id": "camera2",
|
||||
"position_mm": [float(x) for x in (t2_cm * 1000.0)],
|
||||
"orientation_deg": {
|
||||
"roll": cam2_roll,
|
||||
"pitch": cam2_pitch,
|
||||
"yaw": cam2_yaw
|
||||
},
|
||||
}
|
||||
],
|
||||
"markers": marker_list
|
||||
}
|
||||
|
||||
try:
|
||||
|
||||
with open(out_json, 'w', encoding='utf-8') as f:
|
||||
json.dump(json_data, f, indent=4)
|
||||
|
||||
print(f"[INFO] Saved JSON poses to '{out_json}'.")
|
||||
|
||||
except Exception as e:
|
||||
print(f"[WARN] Could not save JSON: {e}")
|
||||
|
||||
# Annotate images with machine axes using camera1 extrinsics
|
||||
try:
|
||||
|
||||
R_machine_to_cam1 = R1_opt
|
||||
t_machine_to_cam1 = t1
|
||||
|
||||
machine_axes = np.float32([
|
||||
[0.0, 0.0, 0.0],
|
||||
[0.200, 0.0, 0.0],
|
||||
[0.0, -0.100, 0.0],
|
||||
[0.0, 0.0, 0.100],
|
||||
])
|
||||
|
||||
rvec_global, _ = cv2.Rodrigues(R_machine_to_cam1)
|
||||
|
||||
imgpts, _ = cv2.projectPoints(
|
||||
machine_axes,
|
||||
rvec_global,
|
||||
t_machine_to_cam1,
|
||||
K1,
|
||||
D1
|
||||
)
|
||||
|
||||
origin = tuple(np.round(imgpts[0].ravel()).astype(int))
|
||||
x_end = tuple(np.round(imgpts[1].ravel()).astype(int))
|
||||
y_end = tuple(np.round(imgpts[2].ravel()).astype(int))
|
||||
z_end = tuple(np.round(imgpts[3].ravel()).astype(int))
|
||||
|
||||
cv2.aruco.drawDetectedMarkers(draw1, corners1)
|
||||
cv2.aruco.drawDetectedMarkers(drawPNG1, corners1)
|
||||
|
||||
try:
|
||||
for i, mid in enumerate(ids1):
|
||||
|
||||
try:
|
||||
pts = corners1[i].reshape((4, 2))
|
||||
center = tuple(np.round(pts.mean(axis=0)).astype(int))
|
||||
except Exception:
|
||||
continue
|
||||
|
||||
text = str(int(mid))
|
||||
|
||||
pos = (
|
||||
int(center[0]) + 15,
|
||||
int(center[1]) - 15
|
||||
)
|
||||
|
||||
cv2.putText(
|
||||
draw1,
|
||||
text,
|
||||
pos,
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
1.0,
|
||||
(255,0,0),
|
||||
3,
|
||||
lineType=cv2.LINE_AA
|
||||
)
|
||||
|
||||
cv2.putText(
|
||||
drawPNG1,
|
||||
text,
|
||||
pos,
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
1.0,
|
||||
(255,0,0,255),
|
||||
3,
|
||||
lineType=cv2.LINE_AA
|
||||
)
|
||||
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
cv2.line(draw1, origin, x_end, (0,0,255), 3)
|
||||
cv2.line(draw1, origin, y_end, (0,255,0), 3)
|
||||
cv2.line(draw1, origin, z_end, (255,0,0), 3)
|
||||
|
||||
cv2.line(drawPNG1, origin, x_end, (0,0,255,255), 3)
|
||||
cv2.line(drawPNG1, origin, y_end, (0,255,0,255), 3)
|
||||
cv2.line(drawPNG1, origin, z_end, (255,0,0,255), 3)
|
||||
|
||||
cv2.putText(draw1, "X (200 mm)", x_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)
|
||||
cv2.putText(draw1, "Y (-100 mm)", y_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
|
||||
cv2.putText(draw1, "+Z (100 mm)", z_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0), 2)
|
||||
|
||||
cv2.putText(drawPNG1, "X (200 mm)", x_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255,255), 2)
|
||||
cv2.putText(drawPNG1, "Y (-100 mm)", y_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0,255), 2)
|
||||
cv2.putText(drawPNG1, "+Z (100 mm)", z_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0,255), 2)
|
||||
|
||||
out_img1 = os.path.join(out_dir, f"{base1}_two_cam_annotated.jpg")
|
||||
|
||||
cv2.imwrite(out_img1, draw1)
|
||||
|
||||
print(f"[INFO] Annotated image saved as '{out_img1}'.")
|
||||
|
||||
gray = cv2.cvtColor(drawPNG1, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
_, alpha = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY)
|
||||
|
||||
drawPNG_1 = cv2.merge([
|
||||
drawPNG1[:, :, 0],
|
||||
drawPNG1[:, :, 1],
|
||||
drawPNG1[:, :, 2],
|
||||
alpha
|
||||
])
|
||||
|
||||
out_png1 = os.path.join(out_dir, f"{base1}_two_cam_overlay.png")
|
||||
|
||||
cv2.imwrite(out_png1, drawPNG_1)
|
||||
|
||||
except Exception as e:
|
||||
print(f"[WARN] Failed to draw machine axes: {e}")
|
||||
|
||||
# Camera 2 overlay
|
||||
try:
|
||||
|
||||
machine_axes2 = np.float32([
|
||||
[0.0, 0.0, 0.0],
|
||||
[0.200, 0.0, 0.0],
|
||||
[0.0, -0.100, 0.0],
|
||||
[0.0, 0.0, 0.100],
|
||||
])
|
||||
|
||||
rvec_global2, _ = cv2.Rodrigues(R2_opt)
|
||||
|
||||
imgpts2, _ = cv2.projectPoints(
|
||||
machine_axes2,
|
||||
rvec_global2,
|
||||
t2,
|
||||
K2,
|
||||
D2
|
||||
)
|
||||
|
||||
origin2 = tuple(np.round(imgpts2[0].ravel()).astype(int))
|
||||
x_end2 = tuple(np.round(imgpts2[1].ravel()).astype(int))
|
||||
y_end2 = tuple(np.round(imgpts2[2].ravel()).astype(int))
|
||||
z_end2 = tuple(np.round(imgpts2[3].ravel()).astype(int))
|
||||
|
||||
cv2.aruco.drawDetectedMarkers(draw2, corners2)
|
||||
cv2.aruco.drawDetectedMarkers(drawPNG2, corners2)
|
||||
|
||||
try:
|
||||
for i, mid in enumerate(ids2):
|
||||
|
||||
try:
|
||||
pts = corners2[i].reshape((4, 2))
|
||||
center = tuple(np.round(pts.mean(axis=0)).astype(int))
|
||||
except Exception:
|
||||
continue
|
||||
|
||||
text = str(int(mid))
|
||||
|
||||
pos = (
|
||||
int(center[0]) + 13,
|
||||
int(center[1]) + 3
|
||||
)
|
||||
|
||||
cv2.putText(
|
||||
draw2,
|
||||
text,
|
||||
pos,
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
1.0,
|
||||
(255,0,0),
|
||||
3,
|
||||
lineType=cv2.LINE_AA
|
||||
)
|
||||
|
||||
cv2.putText(
|
||||
drawPNG2,
|
||||
text,
|
||||
pos,
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
1.0,
|
||||
(255,0,0,255),
|
||||
3,
|
||||
lineType=cv2.LINE_AA
|
||||
)
|
||||
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
cv2.line(draw2, origin2, x_end2, (0,0,255), 3)
|
||||
cv2.line(draw2, origin2, y_end2, (0,255,0), 3)
|
||||
cv2.line(draw2, origin2, z_end2, (255,0,0), 3)
|
||||
|
||||
cv2.line(drawPNG2, origin2, x_end2, (0,0,255,255), 3)
|
||||
cv2.line(drawPNG2, origin2, y_end2, (0,255,0,255), 3)
|
||||
cv2.line(drawPNG2, origin2, z_end2, (255,0,0,255), 3)
|
||||
|
||||
cv2.putText(draw2, "X (200 mm)", x_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)
|
||||
cv2.putText(draw2, "Y (-100 mm)", y_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
|
||||
cv2.putText(draw2, "+Z (100 mm)", z_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0), 2)
|
||||
|
||||
cv2.putText(drawPNG2, "X (200 mm)", x_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255,255), 2)
|
||||
cv2.putText(drawPNG2, "Y (-100 mm)", y_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0,255), 2)
|
||||
cv2.putText(drawPNG2, "+Z (100 mm)", z_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0,255), 2)
|
||||
|
||||
out_img2 = os.path.join(out_dir, f"{base2}_two_cam_annotated.jpg")
|
||||
|
||||
cv2.imwrite(out_img2, draw2)
|
||||
|
||||
print(f"[INFO] Annotated image saved as '{out_img2}'.")
|
||||
|
||||
gray2 = cv2.cvtColor(drawPNG2, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
_, alpha2 = cv2.threshold(gray2, 0, 255, cv2.THRESH_BINARY)
|
||||
|
||||
drawPNG_2 = cv2.merge([
|
||||
drawPNG2[:, :, 0],
|
||||
drawPNG2[:, :, 1],
|
||||
drawPNG2[:, :, 2],
|
||||
alpha2
|
||||
])
|
||||
|
||||
out_png2 = os.path.join(out_dir, f"{base2}_two_cam_overlay.png")
|
||||
|
||||
cv2.imwrite(out_png2, drawPNG_2)
|
||||
|
||||
print(f"[INFO] Overlay PNG saved as '{out_png2}'.")
|
||||
|
||||
except Exception as e:
|
||||
print(f"[WARN] Failed to draw machine axes for camera2: {e}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
BIN
pipeline/render_2d.png
Normal file
BIN
pipeline/render_2d.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 MiB |
BIN
pipeline/render_3a.png
Normal file
BIN
pipeline/render_3a.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
20116
pipeline/render_3a_aruco_detection.json
Normal file
20116
pipeline/render_3a_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
329
pipeline/render_3a_camera_pose.json
Normal file
329
pipeline/render_3a_camera_pose.json
Normal file
@@ -0,0 +1,329 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-29T05:34:16Z",
|
||||
"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_matrix": [
|
||||
[
|
||||
1777.77783203125,
|
||||
0.0,
|
||||
640.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1500.0,
|
||||
360.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 4,
|
||||
"used_marker_ids": [
|
||||
210,
|
||||
205,
|
||||
206,
|
||||
207
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3,
|
||||
4,
|
||||
5,
|
||||
6,
|
||||
7,
|
||||
8,
|
||||
9,
|
||||
10,
|
||||
11,
|
||||
12,
|
||||
13,
|
||||
14,
|
||||
15,
|
||||
16,
|
||||
17,
|
||||
18,
|
||||
19,
|
||||
20,
|
||||
21,
|
||||
22,
|
||||
23,
|
||||
24,
|
||||
25,
|
||||
26,
|
||||
27
|
||||
],
|
||||
"rms": [
|
||||
1.0114134663676055,
|
||||
0.6639182528097215,
|
||||
0.5739469492087472,
|
||||
0.5286835626522526,
|
||||
0.4694051705223743,
|
||||
0.3969964464975006,
|
||||
0.3151761230050886,
|
||||
0.22825811420039874,
|
||||
0.14334708153431971,
|
||||
0.07618149778337073,
|
||||
0.04802982967284346,
|
||||
0.04035287824536157,
|
||||
0.033959902522562355,
|
||||
0.028057435706365975,
|
||||
0.022767087572099415,
|
||||
0.018281990481919433,
|
||||
0.01422702734760535,
|
||||
0.01058775888038278,
|
||||
0.007566043482152367,
|
||||
0.005123936904466261,
|
||||
0.003261157779092087,
|
||||
0.0018882632118665536,
|
||||
0.0007684323923658777,
|
||||
0.0001398894364081004,
|
||||
4.2590187754836205e-05,
|
||||
4.161871278906256e-05,
|
||||
4.161660887281736e-05,
|
||||
4.16166073463803e-05
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125,
|
||||
6.25e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
7.8125e-06,
|
||||
3.90625e-06,
|
||||
1.953125e-06,
|
||||
9.765625e-07,
|
||||
4.8828125e-07,
|
||||
2.44140625e-07,
|
||||
1.220703125e-07,
|
||||
6.103515625e-08,
|
||||
3.0517578125e-08,
|
||||
1.52587890625e-08,
|
||||
7.62939453125e-09,
|
||||
3.814697265625e-09,
|
||||
1.9073486328125e-09,
|
||||
9.5367431640625e-10,
|
||||
4.76837158203125e-10,
|
||||
2.384185791015625e-10,
|
||||
1.1920928955078125e-10,
|
||||
5.960464477539063e-11,
|
||||
2.980232238769531e-11,
|
||||
1.4901161193847657e-11,
|
||||
7.450580596923828e-12
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.10430687360172344,
|
||||
"residual_median_px": 0.07433889289506992,
|
||||
"residual_max_px": 0.1640622076531485,
|
||||
"sigma2_normalized": 6.92776802797649e-09
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7620593309402466,
|
||||
-0.647443413734436,
|
||||
0.009086735546588898
|
||||
],
|
||||
[
|
||||
-0.27515384554862976,
|
||||
-0.33650344610214233,
|
||||
-0.9005863666534424
|
||||
],
|
||||
[
|
||||
0.5861364603042603,
|
||||
0.6837999820709229,
|
||||
-0.4345821440219879
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-273.4744567871094,
|
||||
192.1861572265625,
|
||||
922.5557250976562
|
||||
],
|
||||
"rvec_rad": [
|
||||
1.9264447184295097,
|
||||
-0.7016308556001063,
|
||||
0.45266440822553355
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
-279.4590148925781,
|
||||
-743.2315063476562,
|
||||
576.491455078125
|
||||
],
|
||||
"position_mm": [
|
||||
-279459.0,
|
||||
-743231.5,
|
||||
576491.4375
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 122.43758392333984,
|
||||
"pitch": -35.88331624307256,
|
||||
"yaw": -19.852935791015625
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
3.9943919468012445e-06,
|
||||
1.1154473298546209e-06,
|
||||
-1.118897886488766e-06,
|
||||
-0.0005765194014531353,
|
||||
0.00041358722245405364,
|
||||
0.0022187093112645048
|
||||
],
|
||||
[
|
||||
1.1154473298546145e-06,
|
||||
8.886589103748773e-07,
|
||||
-2.3371494827736547e-06,
|
||||
-0.0005830464121191728,
|
||||
0.00045167407920918234,
|
||||
0.0022451714145222145
|
||||
],
|
||||
[
|
||||
-1.1188978864887337e-06,
|
||||
-2.337149482773653e-06,
|
||||
7.81346071908241e-06,
|
||||
0.0017024049446432569,
|
||||
-0.0013917849716835104,
|
||||
-0.006611198495461098
|
||||
],
|
||||
[
|
||||
-0.0005765194014531287,
|
||||
-0.0005830464121191726,
|
||||
0.0017024049446432584,
|
||||
0.4034039987420133,
|
||||
-0.3201198249762993,
|
||||
-1.5516686677679883
|
||||
],
|
||||
[
|
||||
0.00041358722245404654,
|
||||
0.0004516740792091824,
|
||||
-0.001391784971683514,
|
||||
-0.3201198249762996,
|
||||
0.264775180218034,
|
||||
1.2494317575832103
|
||||
],
|
||||
[
|
||||
0.002218709311264482,
|
||||
0.002245171414522214,
|
||||
-0.006611198495461105,
|
||||
-1.5516686677679885,
|
||||
1.2494317575832097,
|
||||
6.052706569330424
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
0.11451120140577908,
|
||||
0.05401199046483257,
|
||||
0.16015641874171344
|
||||
],
|
||||
"tvec_std_m": [
|
||||
0.6351409282529455,
|
||||
0.51456309644011,
|
||||
2.460224902184844
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
2.1995968929483474,
|
||||
1.1548044194575717,
|
||||
1.510385159357917
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
2199.5968929483474,
|
||||
1154.8044194575718,
|
||||
1510.385159357917
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 0.08832300327411201,
|
||||
"pitch": 0.13890967443140134,
|
||||
"yaw": 0.020881373905939325
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
166.25,
|
||||
674.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
166.26881408691406,
|
||||
674.7455444335938
|
||||
],
|
||||
"reprojection_error_px": 0.019334475384928378,
|
||||
"confidence": 0.23718801109435655
|
||||
},
|
||||
{
|
||||
"marker_id": 205,
|
||||
"observed_center_px": [
|
||||
1127.0,
|
||||
378.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1127.124267578125,
|
||||
378.265380859375
|
||||
],
|
||||
"reprojection_error_px": 0.12521582091799144,
|
||||
"confidence": 0.19676029488014599
|
||||
},
|
||||
{
|
||||
"marker_id": 206,
|
||||
"observed_center_px": [
|
||||
953.25,
|
||||
379.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
953.086181640625,
|
||||
379.0089416503906
|
||||
],
|
||||
"reprojection_error_px": 0.1640622076531485,
|
||||
"confidence": 0.23511362818048806
|
||||
},
|
||||
{
|
||||
"marker_id": 207,
|
||||
"observed_center_px": [
|
||||
1039.5,
|
||||
347.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
1039.5140380859375,
|
||||
347.731201171875
|
||||
],
|
||||
"reprojection_error_px": 0.023461964872148418,
|
||||
"confidence": 0.18076863774153512
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
pipeline/render_3b.png
Normal file
BIN
pipeline/render_3b.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
15562
pipeline/render_3b_aruco_detection.json
Normal file
15562
pipeline/render_3b_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
400
pipeline/render_3b_camera_pose.json
Normal file
400
pipeline/render_3b_camera_pose.json
Normal file
@@ -0,0 +1,400 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-29T05:34:16Z",
|
||||
"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_matrix": [
|
||||
[
|
||||
1777.77783203125,
|
||||
0.0,
|
||||
640.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1500.0,
|
||||
360.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 5,
|
||||
"used_marker_ids": [
|
||||
208,
|
||||
215,
|
||||
214,
|
||||
211,
|
||||
210
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3,
|
||||
4,
|
||||
5,
|
||||
6,
|
||||
7,
|
||||
8,
|
||||
9,
|
||||
10,
|
||||
11,
|
||||
12,
|
||||
13,
|
||||
14,
|
||||
15,
|
||||
16,
|
||||
17,
|
||||
18,
|
||||
19,
|
||||
20,
|
||||
21,
|
||||
22,
|
||||
23,
|
||||
24,
|
||||
25,
|
||||
26,
|
||||
27,
|
||||
28,
|
||||
29,
|
||||
30,
|
||||
31,
|
||||
32,
|
||||
33,
|
||||
34,
|
||||
35,
|
||||
36,
|
||||
37,
|
||||
38,
|
||||
39,
|
||||
40,
|
||||
41,
|
||||
42,
|
||||
43,
|
||||
44,
|
||||
45,
|
||||
46
|
||||
],
|
||||
"rms": [
|
||||
14.818157274424525,
|
||||
6.6429684472286725,
|
||||
3.508293175019171,
|
||||
1.9569659577739633,
|
||||
1.0760750577807272,
|
||||
0.6558111727688115,
|
||||
0.5974384941473672,
|
||||
0.45776430515731414,
|
||||
0.3406747506535104,
|
||||
0.21284192970803506,
|
||||
0.21284192970803506,
|
||||
0.21284192970803506,
|
||||
0.21284192970803506,
|
||||
0.21284192970803506,
|
||||
0.13834869751593928,
|
||||
0.13834869751593928,
|
||||
0.12196764428961278,
|
||||
0.12196764428961278,
|
||||
0.12196764428961278,
|
||||
0.11109006687857191,
|
||||
0.11109006687857191,
|
||||
0.11109006687857191,
|
||||
0.10513254832847944,
|
||||
0.10513254832847944,
|
||||
0.10009463133300968,
|
||||
0.0972011239152783,
|
||||
0.08965747726813587,
|
||||
0.07896966206278197,
|
||||
0.06848004274985553,
|
||||
0.058568387282928645,
|
||||
0.04925640249856142,
|
||||
0.040730620366500596,
|
||||
0.033135104063634416,
|
||||
0.02650247419978119,
|
||||
0.020790086792429628,
|
||||
0.015941755983382246,
|
||||
0.012075496055238399,
|
||||
0.010697420477957333,
|
||||
0.010697420477957333,
|
||||
0.010697420477957333,
|
||||
0.010249775295046573,
|
||||
0.010249775295046573,
|
||||
0.010217299105490024,
|
||||
0.003080951472807064,
|
||||
0.0029219478792723522,
|
||||
0.002921843589954249,
|
||||
0.0029218434666077736
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125,
|
||||
6.25e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
7.8125e-06,
|
||||
3.90625e-06,
|
||||
1.953125e-06,
|
||||
3.90625e-06,
|
||||
7.8125e-06,
|
||||
1.5625e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
3.125e-05,
|
||||
6.25e-05,
|
||||
3.125e-05,
|
||||
6.25e-05,
|
||||
0.000125,
|
||||
6.25e-05,
|
||||
0.000125,
|
||||
6.25e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
7.8125e-06,
|
||||
3.90625e-06,
|
||||
1.953125e-06,
|
||||
9.765625e-07,
|
||||
4.8828125e-07,
|
||||
2.44140625e-07,
|
||||
1.220703125e-07,
|
||||
6.103515625e-08,
|
||||
3.0517578125e-08,
|
||||
1.52587890625e-08,
|
||||
7.62939453125e-09,
|
||||
1.52587890625e-08,
|
||||
3.0517578125e-08,
|
||||
1.52587890625e-08,
|
||||
3.0517578125e-08,
|
||||
1.52587890625e-08,
|
||||
7.62939453125e-09,
|
||||
3.814697265625e-09,
|
||||
1.9073486328125e-09,
|
||||
9.5367431640625e-10
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 7.325988977382018,
|
||||
"residual_median_px": 5.2610158050715405,
|
||||
"residual_max_px": 11.95199194038243,
|
||||
"sigma2_normalized": 2.1342923107320546e-05
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
-0.9725239872932434,
|
||||
0.033903129398822784,
|
||||
-0.23032091557979584
|
||||
],
|
||||
[
|
||||
-0.10051494091749191,
|
||||
0.8312227725982666,
|
||||
0.5467773079872131
|
||||
],
|
||||
[
|
||||
0.20998544991016388,
|
||||
0.5549046993255615,
|
||||
-0.8049763441085815
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
180.4925537109375,
|
||||
-45.28935623168945,
|
||||
-859.5316162109375
|
||||
],
|
||||
"rvec_rad": [
|
||||
-0.05955419353489003,
|
||||
3.226373471557783,
|
||||
0.9849571463938018
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
351.47021484375,
|
||||
508.4844055175781,
|
||||
-625.5681762695312
|
||||
],
|
||||
"position_mm": [
|
||||
351470.21875,
|
||||
508484.40625,
|
||||
-625568.1875
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 145.41983032226562,
|
||||
"pitch": -12.121499395069357,
|
||||
"yaw": -174.09915161132812
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
0.002350044145762458,
|
||||
0.0018448663795067805,
|
||||
0.002791262415386132,
|
||||
0.044346229182918716,
|
||||
-0.07916151257588579,
|
||||
-0.14927480851129382
|
||||
],
|
||||
[
|
||||
0.0018448663795067872,
|
||||
0.0036429204877860334,
|
||||
-0.001238434463788633,
|
||||
0.006197854651266595,
|
||||
0.030206644893667412,
|
||||
-0.35477216802776246
|
||||
],
|
||||
[
|
||||
0.002791262415386112,
|
||||
-0.00123843446378866,
|
||||
0.015531235537186557,
|
||||
0.08030698335197134,
|
||||
-0.04481294954598946,
|
||||
0.2747388184172584
|
||||
],
|
||||
[
|
||||
0.04434622918291879,
|
||||
0.006197854651266368,
|
||||
0.08030698335197246,
|
||||
5.115947766978903,
|
||||
-1.7296342108112102,
|
||||
11.501531197034321
|
||||
],
|
||||
[
|
||||
-0.07916151257588577,
|
||||
0.030206644893667495,
|
||||
-0.044812949545989626,
|
||||
-1.7296342108112113,
|
||||
17.275134170996235,
|
||||
15.481414615323398
|
||||
],
|
||||
[
|
||||
-0.14927480851129452,
|
||||
-0.3547721680277628,
|
||||
0.2747388184172568,
|
||||
11.5015311970343,
|
||||
15.481414615323407,
|
||||
179.23527748073005
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
2.777542050899846,
|
||||
3.458179074295007,
|
||||
7.14045087126885
|
||||
],
|
||||
"tvec_std_m": [
|
||||
2.261846097102741,
|
||||
4.156336628690731,
|
||||
13.387878005148167
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
52.59102600708334,
|
||||
50.66008686135971,
|
||||
43.3784726294464
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
52591.02600708335,
|
||||
50660.08686135971,
|
||||
43378.4726294464
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 4.057851126238033,
|
||||
"pitch": 3.771174837779904,
|
||||
"yaw": 1.7875669447691551
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 208,
|
||||
"observed_center_px": [
|
||||
995.5,
|
||||
638.0
|
||||
],
|
||||
"projected_center_px": [
|
||||
986.5569458007812,
|
||||
638.244384765625
|
||||
],
|
||||
"reprojection_error_px": 8.946392698950438,
|
||||
"confidence": 0.6072727689079809
|
||||
},
|
||||
{
|
||||
"marker_id": 215,
|
||||
"observed_center_px": [
|
||||
764.5,
|
||||
612.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
776.3766479492188,
|
||||
613.8399047851562
|
||||
],
|
||||
"reprojection_error_px": 11.95199194038243,
|
||||
"confidence": 0.6168610191628993
|
||||
},
|
||||
{
|
||||
"marker_id": 214,
|
||||
"observed_center_px": [
|
||||
996.0,
|
||||
527.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
999.8966064453125,
|
||||
527.873291015625
|
||||
],
|
||||
"reprojection_error_px": 3.898556459022334,
|
||||
"confidence": 0.5524643209674667
|
||||
},
|
||||
{
|
||||
"marker_id": 211,
|
||||
"observed_center_px": [
|
||||
783.0,
|
||||
506.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
777.8927612304688,
|
||||
504.9873046875
|
||||
],
|
||||
"reprojection_error_px": 5.2610158050715405,
|
||||
"confidence": 0.5730650050844824
|
||||
},
|
||||
{
|
||||
"marker_id": 210,
|
||||
"observed_center_px": [
|
||||
312.75,
|
||||
470.75
|
||||
],
|
||||
"projected_center_px": [
|
||||
311.1917419433594,
|
||||
470.3533630371094
|
||||
],
|
||||
"reprojection_error_px": 1.6079455996446261,
|
||||
"confidence": 0.5452330001368535
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
BIN
pipeline/render_3b_two_cam_annotated.jpg
Normal file
BIN
pipeline/render_3b_two_cam_annotated.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 278 KiB |
BIN
pipeline/render_3b_two_cam_overlay.png
Normal file
BIN
pipeline/render_3b_two_cam_overlay.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 24 KiB |
BIN
pipeline/render_3c.png
Normal file
BIN
pipeline/render_3c.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.2 MiB |
9495
pipeline/render_3c_aruco_detection.json
Normal file
9495
pipeline/render_3c_aruco_detection.json
Normal file
File diff suppressed because it is too large
Load Diff
390
pipeline/render_3c_camera_pose.json
Normal file
390
pipeline/render_3c_camera_pose.json
Normal file
@@ -0,0 +1,390 @@
|
||||
{
|
||||
"schema_version": "1.0",
|
||||
"created_utc": "2026-05-29T05:34:17Z",
|
||||
"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"
|
||||
},
|
||||
"camera": {
|
||||
"camera_id": "cam1",
|
||||
"camera_matrix": [
|
||||
[
|
||||
1777.77783203125,
|
||||
0.0,
|
||||
640.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1500.0,
|
||||
360.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"distortion_coefficients": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
},
|
||||
"estimation": {
|
||||
"method": "single_camera_marker_center_lm",
|
||||
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
|
||||
"marker_size_m": 0.025,
|
||||
"num_used_markers": 3,
|
||||
"used_marker_ids": [
|
||||
214,
|
||||
215,
|
||||
211
|
||||
],
|
||||
"history": {
|
||||
"iters": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3,
|
||||
4,
|
||||
5,
|
||||
6,
|
||||
7,
|
||||
8,
|
||||
9,
|
||||
10,
|
||||
11,
|
||||
12,
|
||||
13,
|
||||
14,
|
||||
15,
|
||||
16,
|
||||
17,
|
||||
18,
|
||||
19,
|
||||
20,
|
||||
21,
|
||||
22,
|
||||
23,
|
||||
24,
|
||||
25,
|
||||
26,
|
||||
27,
|
||||
28,
|
||||
29,
|
||||
30,
|
||||
31,
|
||||
32,
|
||||
33,
|
||||
34,
|
||||
35,
|
||||
36,
|
||||
37,
|
||||
38,
|
||||
39,
|
||||
40,
|
||||
41,
|
||||
42,
|
||||
43,
|
||||
44,
|
||||
45,
|
||||
46,
|
||||
47,
|
||||
48,
|
||||
49,
|
||||
50,
|
||||
51,
|
||||
52
|
||||
],
|
||||
"rms": [
|
||||
8.276652092351446,
|
||||
3.2695419276393194,
|
||||
1.6997554577172982,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.9031353472765118,
|
||||
0.8761012206869034,
|
||||
0.8761012206869034,
|
||||
0.8761012206869034,
|
||||
0.6657758310157231,
|
||||
0.4982919577482978,
|
||||
0.463899810578019,
|
||||
0.4620191377323524,
|
||||
0.46061835425038966,
|
||||
0.45820325758845926,
|
||||
0.4535628754664195,
|
||||
0.44440988606652543,
|
||||
0.42668189732885436,
|
||||
0.39303163296273724,
|
||||
0.33250807541862465,
|
||||
0.2873217198737302,
|
||||
0.2310754990881048,
|
||||
0.1683327418729557,
|
||||
0.12140389664238704,
|
||||
0.091031135899403,
|
||||
0.06974953807273628,
|
||||
0.05302622844733477,
|
||||
0.039719619692176966,
|
||||
0.037288115046028705,
|
||||
0.019479927098974645,
|
||||
0.009348648211600379,
|
||||
0.005670126166140046,
|
||||
0.004435173697432046,
|
||||
0.003309583557122678,
|
||||
0.0021075079585650964,
|
||||
0.0010750386520058455,
|
||||
0.00038672742677653426,
|
||||
8.049312829495705e-05,
|
||||
9.15644812387087e-06,
|
||||
5.813634591654259e-07,
|
||||
1.9407836813260934e-08,
|
||||
3.2977852013459893e-10,
|
||||
2.8258286470569123e-12,
|
||||
1.2165664095469026e-14
|
||||
],
|
||||
"lambda": [
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125,
|
||||
0.00025,
|
||||
0.0005,
|
||||
0.001,
|
||||
0.002,
|
||||
0.004,
|
||||
0.008,
|
||||
0.016,
|
||||
0.032,
|
||||
0.064,
|
||||
0.128,
|
||||
0.256,
|
||||
0.128,
|
||||
0.256,
|
||||
0.512,
|
||||
0.256,
|
||||
0.128,
|
||||
0.064,
|
||||
0.032,
|
||||
0.016,
|
||||
0.008,
|
||||
0.004,
|
||||
0.002,
|
||||
0.001,
|
||||
0.0005,
|
||||
0.00025,
|
||||
0.000125,
|
||||
6.25e-05,
|
||||
3.125e-05,
|
||||
1.5625e-05,
|
||||
7.8125e-06,
|
||||
3.90625e-06,
|
||||
1.953125e-06,
|
||||
9.765625e-07,
|
||||
4.8828125e-07,
|
||||
2.44140625e-07,
|
||||
1.220703125e-07,
|
||||
6.103515625e-08,
|
||||
3.0517578125e-08,
|
||||
1.52587890625e-08,
|
||||
7.62939453125e-09,
|
||||
3.814697265625e-09,
|
||||
1.9073486328125e-09,
|
||||
9.5367431640625e-10,
|
||||
4.76837158203125e-10,
|
||||
2.384185791015625e-10,
|
||||
1.1920928955078125e-10,
|
||||
5.960464477539063e-11,
|
||||
2.980232238769531e-11,
|
||||
1.4901161193847657e-11
|
||||
]
|
||||
},
|
||||
"residual_rms_px": 0.0,
|
||||
"residual_median_px": 0.0,
|
||||
"residual_max_px": 0.0,
|
||||
"sigma2_normalized": 9.648909021380052e-32
|
||||
},
|
||||
"camera_pose": {
|
||||
"world_to_camera": {
|
||||
"rotation_matrix": [
|
||||
[
|
||||
0.7478234767913818,
|
||||
0.5858157873153687,
|
||||
-0.3123779296875
|
||||
],
|
||||
[
|
||||
0.6608203053474426,
|
||||
-0.6115613579750061,
|
||||
0.43509677052497864
|
||||
],
|
||||
[
|
||||
0.06384828686714172,
|
||||
-0.5318012237548828,
|
||||
-0.8444588780403137
|
||||
]
|
||||
],
|
||||
"translation_m": [
|
||||
-38.69017028808594,
|
||||
-76.18257904052734,
|
||||
733.07861328125
|
||||
],
|
||||
"rvec_rad": [
|
||||
3.428568125507585,
|
||||
1.3340777852363335,
|
||||
-0.2659620236785496
|
||||
]
|
||||
},
|
||||
"camera_in_world": {
|
||||
"position_m": [
|
||||
32.47060012817383,
|
||||
365.9270935058594,
|
||||
640.1156005859375
|
||||
],
|
||||
"position_mm": [
|
||||
32470.599609375,
|
||||
365927.09375,
|
||||
640115.625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -147.79916381835938,
|
||||
"pitch": -3.660727574305826,
|
||||
"yaw": 41.46568298339844
|
||||
}
|
||||
},
|
||||
"uncertainty": {
|
||||
"pose_covariance_6x6": [
|
||||
[
|
||||
8.629612631969329e-29,
|
||||
6.419343182143809e-30,
|
||||
1.7908295924670243e-29,
|
||||
-7.073499368795134e-27,
|
||||
5.6187259819083295e-27,
|
||||
-2.0666865847138868e-26
|
||||
],
|
||||
[
|
||||
6.419343182143446e-30,
|
||||
3.051760893870683e-29,
|
||||
4.68511451162592e-29,
|
||||
1.5261835153152037e-27,
|
||||
-5.226262008292569e-28,
|
||||
-1.103919177657209e-26
|
||||
],
|
||||
[
|
||||
1.7908295924670055e-29,
|
||||
4.685114511625924e-29,
|
||||
2.0846170216876133e-28,
|
||||
6.842689966147899e-29,
|
||||
3.484907360650631e-27,
|
||||
-3.678530361558646e-26
|
||||
],
|
||||
[
|
||||
-7.073499368795157e-27,
|
||||
1.5261835153151694e-27,
|
||||
6.842689966146321e-29,
|
||||
8.882557859237887e-25,
|
||||
-4.5534571307576935e-25,
|
||||
1.9082917888723267e-24
|
||||
],
|
||||
[
|
||||
5.618725981908361e-27,
|
||||
-5.226262008292345e-28,
|
||||
3.484907360650649e-27,
|
||||
-4.553457130757711e-25,
|
||||
6.148749838486226e-25,
|
||||
-9.837708583024273e-25
|
||||
],
|
||||
[
|
||||
-2.0666865847138762e-26,
|
||||
-1.1039191776572181e-26,
|
||||
-3.678530361558645e-26,
|
||||
1.9082917888723113e-24,
|
||||
-9.837708583024117e-25,
|
||||
1.3630824927229218e-23
|
||||
]
|
||||
],
|
||||
"parameter_std": {
|
||||
"rvec_std_deg": [
|
||||
5.322532023987312e-13,
|
||||
3.1651761469334046e-13,
|
||||
8.272480883107961e-13
|
||||
],
|
||||
"tvec_std_m": [
|
||||
9.424732282265575e-13,
|
||||
7.841396456299239e-13,
|
||||
3.691994708450869e-12
|
||||
]
|
||||
},
|
||||
"camera_center_std_m": [
|
||||
6.236013873929964e-12,
|
||||
5.578343371266709e-12,
|
||||
2.895045264319711e-12
|
||||
],
|
||||
"camera_center_std_mm": [
|
||||
6.2360138739299636e-09,
|
||||
5.578343371266708e-09,
|
||||
2.8950452643197107e-09
|
||||
],
|
||||
"orientation_std_deg": {
|
||||
"roll": 4.550535817991357e-13,
|
||||
"pitch": 4.763282335043431e-13,
|
||||
"yaw": 1.775861640110364e-13
|
||||
}
|
||||
}
|
||||
},
|
||||
"observations": {
|
||||
"markers": [
|
||||
{
|
||||
"marker_id": 214,
|
||||
"observed_center_px": [
|
||||
1147.5,
|
||||
678.25
|
||||
],
|
||||
"projected_center_px": [
|
||||
1147.5,
|
||||
678.25
|
||||
],
|
||||
"reprojection_error_px": 0.0,
|
||||
"confidence": 0.1700115058329765
|
||||
},
|
||||
{
|
||||
"marker_id": 215,
|
||||
"observed_center_px": [
|
||||
853.0,
|
||||
631.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
853.0,
|
||||
631.5
|
||||
],
|
||||
"reprojection_error_px": 0.0,
|
||||
"confidence": 0.9372326698512834
|
||||
},
|
||||
{
|
||||
"marker_id": 211,
|
||||
"observed_center_px": [
|
||||
975.5,
|
||||
549.5
|
||||
],
|
||||
"projected_center_px": [
|
||||
975.5,
|
||||
549.5
|
||||
],
|
||||
"reprojection_error_px": 0.0,
|
||||
"confidence": 0.8902520865668376
|
||||
}
|
||||
]
|
||||
},
|
||||
"qa": {
|
||||
"sanity_notes": []
|
||||
}
|
||||
}
|
||||
17
pipeline/render_3c_two_cam.csv
Normal file
17
pipeline/render_3c_two_cam.csv
Normal file
@@ -0,0 +1,17 @@
|
||||
id,x_mm,y_mm,z_mm,roll_deg,pitch_deg,yaw_deg,seen_by
|
||||
camera 0,462.85,-447.38,679.21,-139.679,-3.166,49.062
|
||||
camera 1,278.74,-619.38,596.01,-128.837,-0.336,9.419
|
||||
122,202.24,-295.10,250.81,-28.414,-15.485,1.697,3
|
||||
198,126.58,-65.51,155.07,-3.661,-5.631,-2.911,3
|
||||
208,351.02,-86.53,-1.04,-1.448,-0.084,0.377,2
|
||||
210,-2.56,36.17,-52.21,1.418,0.510,0.839,2
|
||||
211,250.00,-10.00,3.00,68.426,25.579,18.293,3
|
||||
214,350.00,-10.00,3.00,-1.702,-0.340,0.760,3
|
||||
215,250.00,-90.00,3.00,2.880,-1.221,-1.765,3
|
||||
218,236.59,-245.24,189.57,-58.103,50.616,-52.876,1
|
||||
219,227.26,-329.71,232.26,-59.187,48.504,-50.906,1
|
||||
229,124.73,-146.21,161.04,-3.612,-3.364,-1.997,3
|
||||
243,122.58,-179.86,116.23,67.866,-8.370,-6.945,2
|
||||
244,246.50,-158.07,135.63,-68.544,55.107,-66.578,1
|
||||
246,179.04,-106.04,108.88,103.363,42.379,32.981,1
|
||||
247,143.22,-106.60,110.01,-28.304,-5.147,-2.319,1
|
||||
|
225
pipeline/render_3c_two_cam.json
Normal file
225
pipeline/render_3c_two_cam.json
Normal file
@@ -0,0 +1,225 @@
|
||||
{
|
||||
"metadata": {
|
||||
"timestamp": "2026-05-29 07:01:38",
|
||||
"reference_markers": [
|
||||
211,
|
||||
215,
|
||||
214
|
||||
],
|
||||
"dict": "DICT_4X4_250",
|
||||
"marker_size_mm": 25.0,
|
||||
"description": "Two-camera joint optimization with triangulation"
|
||||
},
|
||||
"cameras": [
|
||||
{
|
||||
"id": "camera1",
|
||||
"position_mm": [
|
||||
462.85337477011745,
|
||||
-447.3761981117614,
|
||||
679.2119813369037
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -139.67859744771047,
|
||||
"pitch": -3.165957561501241,
|
||||
"yaw": 49.06230550849313
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "camera2",
|
||||
"position_mm": [
|
||||
278.74292453701025,
|
||||
-619.3816023355497,
|
||||
596.013403520274
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -128.8365839999356,
|
||||
"pitch": -0.33589695340564535,
|
||||
"yaw": 9.419081380244078
|
||||
}
|
||||
}
|
||||
],
|
||||
"markers": [
|
||||
{
|
||||
"id": 122,
|
||||
"position_mm": [
|
||||
202.24261474609375,
|
||||
-295.09710693359375,
|
||||
250.81494140625
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -28.41441452710221,
|
||||
"pitch": -15.485467478547452,
|
||||
"yaw": 1.6970935429672367
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 198,
|
||||
"position_mm": [
|
||||
126.584228515625,
|
||||
-65.51023864746094,
|
||||
155.06854248046875
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -3.660633208127074,
|
||||
"pitch": -5.630630889795624,
|
||||
"yaw": -2.9106432706678698
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 208,
|
||||
"position_mm": [
|
||||
351.0157443651476,
|
||||
-86.53427022011029,
|
||||
-1.0397490394067344
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -1.4484575811662157,
|
||||
"pitch": -0.08375253351626927,
|
||||
"yaw": 0.37677851429577486
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 210,
|
||||
"position_mm": [
|
||||
-2.558166926849015,
|
||||
36.16561975926857,
|
||||
-52.21100279675572
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 1.4182725874800994,
|
||||
"pitch": 0.5101505675200076,
|
||||
"yaw": 0.8388746917381826
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 211,
|
||||
"position_mm": [
|
||||
250.00003051757812,
|
||||
-10.000012397766113,
|
||||
3.0000078678131104
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 68.42584362262588,
|
||||
"pitch": 25.578734156462517,
|
||||
"yaw": 18.29336709620276
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 214,
|
||||
"position_mm": [
|
||||
350.0,
|
||||
-9.999968528747559,
|
||||
2.999967336654663
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -1.7021011527964998,
|
||||
"pitch": -0.34049805661256133,
|
||||
"yaw": 0.7603837134528649
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 215,
|
||||
"position_mm": [
|
||||
250.0,
|
||||
-90.00000762939453,
|
||||
3.000002145767212
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 2.879885512023364,
|
||||
"pitch": -1.2207582699857618,
|
||||
"yaw": -1.7652611699290457
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 218,
|
||||
"position_mm": [
|
||||
236.59325735949972,
|
||||
-245.23626778778672,
|
||||
189.57266262534728
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -58.10323420416955,
|
||||
"pitch": 50.61599751527859,
|
||||
"yaw": -52.875746653538656
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 219,
|
||||
"position_mm": [
|
||||
227.2649738848997,
|
||||
-329.7144822031666,
|
||||
232.2572485343396
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -59.18660506982057,
|
||||
"pitch": 48.503782448589995,
|
||||
"yaw": -50.906238173493115
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 229,
|
||||
"position_mm": [
|
||||
124.72924041748047,
|
||||
-146.21078491210938,
|
||||
161.03932189941406
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -3.6119417177489486,
|
||||
"pitch": -3.3643207255933993,
|
||||
"yaw": -1.9966876167092664
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 243,
|
||||
"position_mm": [
|
||||
122.57701769337837,
|
||||
-179.85790574360627,
|
||||
116.23497462026722
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 67.86582375271519,
|
||||
"pitch": -8.37046200567875,
|
||||
"yaw": -6.945220224171688
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 244,
|
||||
"position_mm": [
|
||||
246.4997454888863,
|
||||
-158.06502830092427,
|
||||
135.62936814972826
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -68.54419573537868,
|
||||
"pitch": 55.10689840067641,
|
||||
"yaw": -66.57844357425445
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 246,
|
||||
"position_mm": [
|
||||
179.03805942690053,
|
||||
-106.03594119519106,
|
||||
108.88294827910639
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": 103.36293213539558,
|
||||
"pitch": 42.37920301941833,
|
||||
"yaw": 32.98079690795832
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": 247,
|
||||
"position_mm": [
|
||||
143.22372509352726,
|
||||
-106.60064380982665,
|
||||
110.0127892418814
|
||||
],
|
||||
"orientation_deg": {
|
||||
"roll": -28.30352249482726,
|
||||
"pitch": -5.146503410532437,
|
||||
"yaw": -2.318781544830814
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
BIN
pipeline/render_3c_two_cam_annotated.jpg
Normal file
BIN
pipeline/render_3c_two_cam_annotated.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 309 KiB |
BIN
pipeline/render_3c_two_cam_overlay.png
Normal file
BIN
pipeline/render_3c_two_cam_overlay.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 33 KiB |
10
pipeline/run_pipeline_3.bat
Normal file
10
pipeline/run_pipeline_3.bat
Normal file
@@ -0,0 +1,10 @@
|
||||
|
||||
python3 1_detect_aruco_observations.py --image render_3a.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_3b.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
python3 1_detect_aruco_observations.py --image render_3c.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir .
|
||||
|
||||
|
||||
|
||||
python3 2_estimate_camera_from_observations.py --robot ../robot.json --i render_3a_aruco_detection.json --outDir "C:/Users/kech/SynologyDrive/2026-AppServer-AppRobot/appRobotRendering/pipeline/"
|
||||
python3 2_estimate_camera_from_observations.py --robot ../robot.json --i render_3b_aruco_detection.json --outDir "C:/Users/kech/SynologyDrive/2026-AppServer-AppRobot/appRobotRendering/pipeline/"
|
||||
python3 2_estimate_camera_from_observations.py --robot ../robot.json --i render_3c_aruco_detection.json --outDir "C:/Users/kech/SynologyDrive/2026-AppServer-AppRobot/appRobotRendering/pipeline/"
|
||||
1
pipeline/run_readTwoMarkers.py
Normal file
1
pipeline/run_readTwoMarkers.py
Normal file
@@ -0,0 +1 @@
|
||||
python3 readTwoImages.py -i render_3c.png -i render_3b.png -npz render.npz -npz render.npz -settings settingsReadTwoMarkers.json
|
||||
16
pipeline/settingsReadTwoMarkers.json
Normal file
16
pipeline/settingsReadTwoMarkers.json
Normal file
@@ -0,0 +1,16 @@
|
||||
{ "coordinateSystem":{
|
||||
"MarkersUsed":"DICT_4X4_250",
|
||||
"KnownMarkers":
|
||||
{
|
||||
"210": [0.020, -0.020, 0.003],
|
||||
"211": [0.250, -0.010, 0.003],
|
||||
"215": [0.250, -0.090, 0.003],
|
||||
"214": [0.350, -0.010, 0.003],
|
||||
"208": [0.350, -0.090, 0.003],
|
||||
"206": [0.650, -0.010, 0.003],
|
||||
"205": [0.750, -0.090, 0.003],
|
||||
"207": [0.750, -0.010, 0.003],
|
||||
"217": [0.650, -0.090, 0.003]
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
render.png
BIN
render.png
Binary file not shown.
|
Before Width: | Height: | Size: 1.3 MiB |
39
robot.json
39
robot.json
@@ -16,7 +16,13 @@
|
||||
"renderingInfo": {
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
"cameraPosition": [370, -600, 500],
|
||||
"cameraPosition__1": [-10, -800, 500],
|
||||
"cameraPosition__2": [-500, 300, 1200],
|
||||
"cameraPosition__3": [-200, -900, 200],
|
||||
"cameraPosition__4": [1200, 200, 300],
|
||||
"cameraPosition_a":[-300, -800,500],
|
||||
"cameraPosition":[300, -700,500],
|
||||
"cameraPosition_c":[600, -500,600],
|
||||
"cameraTarget": [210, -180, 180],
|
||||
"cameraUpVector": [0, 0, 1],
|
||||
"lightPosition": [-500, -500, 500],
|
||||
@@ -78,10 +84,19 @@
|
||||
}
|
||||
},
|
||||
"defaultPosition": {
|
||||
"x": 100,
|
||||
"y": 30,
|
||||
"z": -30,
|
||||
"a": -120,
|
||||
"x": 10,
|
||||
"y": 4,
|
||||
"z": 20,
|
||||
"a": 10,
|
||||
"b": 2,
|
||||
"c": 9,
|
||||
"e": 1
|
||||
},
|
||||
"defaultPosition____": {
|
||||
"x": 140,
|
||||
"y": 46,
|
||||
"z": -70,
|
||||
"a": 120,
|
||||
"b": 22,
|
||||
"c": 91,
|
||||
"e": 10
|
||||
@@ -95,22 +110,22 @@
|
||||
"c": null,
|
||||
"e": null
|
||||
},
|
||||
"multiview_quality": {
|
||||
"multiview_calculation": {
|
||||
"combine_mode": "mean",
|
||||
"size_ref_px": 50.0,
|
||||
"border_ref_px": 120.0,
|
||||
"center_ref_norm": 0.01,
|
||||
"sharpness_ref": 2500.0,
|
||||
"homography_ref": 0.18,
|
||||
"size_factor": 0.01,
|
||||
"aspect_factor": 0.01,
|
||||
"size_factor": 0.3,
|
||||
"aspect_factor": 0.3,
|
||||
"border_factor": 0.01,
|
||||
"center_factor": 0.01,
|
||||
"sharpness_factor": 0.01,
|
||||
"homography_factor": 0.01,
|
||||
"sharpness_factor": 0.5,
|
||||
"homography_factor": 0.2,
|
||||
"normal_visibility_factor": 0.01,
|
||||
"spin_factor": 0.01,
|
||||
"weight_floor": 0.01
|
||||
"spin_factor": 0.3,
|
||||
"weight_floor": 0.3
|
||||
},
|
||||
"movements": {
|
||||
"x": null,
|
||||
|
||||
2351
vibeCoding/29.5.txt
2351
vibeCoding/29.5.txt
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user