Arbeiten mit GPT
This commit is contained in:
chk
@@ -16,6 +16,17 @@ import numpy as np
|
|||||||
# Utilities
|
# 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):
|
def load_intrinsics_npz(npz_path: str):
|
||||||
data = np.load(npz_path)
|
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):
|
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:
|
with open(robot_json_path, 'r', encoding='utf-8') as f:
|
||||||
robot = json.load(f)
|
robot = json.load(f)
|
||||||
|
|
||||||
@@ -339,7 +353,9 @@ def main():
|
|||||||
|
|
||||||
args = parser.parse_args()
|
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
|
# Load robot vision config
|
||||||
@@ -354,7 +370,10 @@ def main():
|
|||||||
# Load image
|
# Load image
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|
||||||
image = cv2.imread(args.image)
|
|
||||||
|
image_path = resolve_path(args.image)
|
||||||
|
image = cv2.imread(image_path)
|
||||||
|
|
||||||
|
|
||||||
if image is None:
|
if image is None:
|
||||||
raise RuntimeError(f'Cannot read image: {args.image}')
|
raise RuntimeError(f'Cannot read image: {args.image}')
|
||||||
@@ -370,7 +389,9 @@ def main():
|
|||||||
# Intrinsics
|
# Intrinsics
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|
||||||
K, D = load_intrinsics_npz(args.intrinsics)
|
|
||||||
|
intrinsics_path = resolve_path(args.intrinsics)
|
||||||
|
K, D = load_intrinsics_npz(intrinsics_path)
|
||||||
|
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
# Detection
|
# Detection
|
||||||
@@ -562,7 +583,7 @@ def main():
|
|||||||
input_base = os.path.splitext(input_filename)[0]
|
input_base = os.path.splitext(input_filename)[0]
|
||||||
|
|
||||||
out_json = os.path.join(
|
out_json = os.path.join(
|
||||||
args.outDir,
|
out_dir,
|
||||||
f'{input_base}_aruco_detection.json'
|
f'{input_base}_aruco_detection.json'
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -170,7 +170,7 @@ class ObservationQualityConfig:
|
|||||||
def _load_nested_quality_config(src: Dict[str, Any]) -> Dict[str, Any]:
|
def _load_nested_quality_config(src: Dict[str, Any]) -> Dict[str, Any]:
|
||||||
if not isinstance(src, dict):
|
if not isinstance(src, dict):
|
||||||
return {}
|
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)
|
v = src.get(key)
|
||||||
if isinstance(v, dict):
|
if isinstance(v, dict):
|
||||||
return v
|
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",
|
"schema_version": "1.0",
|
||||||
"created_utc": "2026-05-28T21:28:20Z",
|
"created_utc": "2026-05-28T21:44:42Z",
|
||||||
"vision_config": {
|
"vision_config": {
|
||||||
"MarkerType": "DICT_4X4_250",
|
"MarkerType": "DICT_4X4_250",
|
||||||
"MarkerSize": 0.025
|
"MarkerSize": 0.025
|
||||||
@@ -46,7 +46,7 @@
|
|||||||
},
|
},
|
||||||
"detections": [
|
"detections": [
|
||||||
{
|
{
|
||||||
"observation_id": "492f34d0-5512-4bf1-9f5c-2e70826f2e71",
|
"observation_id": "4d04662e-5080-4098-8fc4-46dce4e705f5",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 102,
|
"marker_id": 102,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -100,7 +100,7 @@
|
|||||||
"confidence": 0.9494437061622057
|
"confidence": 0.9494437061622057
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "f517029a-9c63-4593-a583-51bbc9991930",
|
"observation_id": "449cb42f-7615-4568-b338-d2f044c6ec16",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 243,
|
"marker_id": 243,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -154,7 +154,7 @@
|
|||||||
"confidence": 0.8899728634608616
|
"confidence": 0.8899728634608616
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "7f05752e-d8d5-48c1-a9de-1d9fe78d34fc",
|
"observation_id": "fc29ae3d-bcba-4cca-9d8b-e23fbe32ead8",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 210,
|
"marker_id": 210,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -208,7 +208,7 @@
|
|||||||
"confidence": 0.5673043275049208
|
"confidence": 0.5673043275049208
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "0f6019cf-ae79-4c58-b42f-454ad8a49b2c",
|
"observation_id": "2c78068c-dce5-4783-af57-f452515adda2",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 247,
|
"marker_id": 247,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -262,7 +262,7 @@
|
|||||||
"confidence": 0.8796777163094818
|
"confidence": 0.8796777163094818
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "b6afa073-1316-4e40-b067-4ae58fa88bca",
|
"observation_id": "bab2a129-54ce-4ea2-b1d7-23977a28b351",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 246,
|
"marker_id": 246,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -316,7 +316,7 @@
|
|||||||
"confidence": 0.8015179238063419
|
"confidence": 0.8015179238063419
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "5b95720f-f38f-4b70-b5a7-2a8407410850",
|
"observation_id": "bb69b260-1543-4e95-80af-b6fee95585aa",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 101,
|
"marker_id": 101,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -370,7 +370,7 @@
|
|||||||
"confidence": 0.3325004465415643
|
"confidence": 0.3325004465415643
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "7dc93dc7-2686-48f6-bae4-c32d18d8a7a8",
|
"observation_id": "e5ba9b93-f275-43e5-ab66-382210ce90d8",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 215,
|
"marker_id": 215,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -424,7 +424,7 @@
|
|||||||
"confidence": 0.7952557920267838
|
"confidence": 0.7952557920267838
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "94d6d6ac-b481-4d2d-a193-1451736ecf89",
|
"observation_id": "4ad2b53e-2dbe-450c-92c7-d06b071cd796",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 124,
|
"marker_id": 124,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -478,7 +478,7 @@
|
|||||||
"confidence": 0.34091855704160245
|
"confidence": 0.34091855704160245
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "7da58bb6-8abb-4cb8-b93a-f19e3b30bab5",
|
"observation_id": "7989f10c-2d6a-44b7-b16d-e721db2e585e",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 229,
|
"marker_id": 229,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -532,7 +532,7 @@
|
|||||||
"confidence": 0.26810902547334975
|
"confidence": 0.26810902547334975
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "c1447225-65f9-4e8e-ac4a-9913fbd3fa4a",
|
"observation_id": "e2db913d-c818-4637-af5e-253d617ca04e",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 122,
|
"marker_id": 122,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -586,7 +586,7 @@
|
|||||||
"confidence": 0.18665602922528673
|
"confidence": 0.18665602922528673
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "c6bf9790-b7cd-4781-addf-593b43be010a",
|
"observation_id": "76503575-cf6f-4ee8-9b3a-311ed41780be",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 198,
|
"marker_id": 198,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -640,7 +640,7 @@
|
|||||||
"confidence": 0.20150745250271476
|
"confidence": 0.20150745250271476
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "ea51fd97-3cc1-450e-8f88-341da29d24fd",
|
"observation_id": "5458000b-97dc-419e-bc20-2cace8792227",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 211,
|
"marker_id": 211,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -694,7 +694,7 @@
|
|||||||
"confidence": 0.6412317666518965
|
"confidence": 0.6412317666518965
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "0f91cfb9-9382-456b-9fe7-2cc966a1fc59",
|
"observation_id": "27ef8edf-4bf1-45cc-80d2-4ae6afc967c8",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 208,
|
"marker_id": 208,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -748,7 +748,7 @@
|
|||||||
"confidence": 0.6280424706698967
|
"confidence": 0.6280424706698967
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "bc4bcbaf-41ff-477e-a495-f1682dcf7c10",
|
"observation_id": "26da9259-8af1-4dc7-b0dc-9f0bf85958c9",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 217,
|
"marker_id": 217,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -802,7 +802,7 @@
|
|||||||
"confidence": 0.35596573288593486
|
"confidence": 0.35596573288593486
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "a11c0669-5498-40dd-ab5d-944be3d29f37",
|
"observation_id": "0caa0146-c0f8-4aaf-b394-95b32436dd0d",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 206,
|
"marker_id": 206,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -856,7 +856,7 @@
|
|||||||
"confidence": 0.33820423087105295
|
"confidence": 0.33820423087105295
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "d0910683-77ca-4ce7-9533-6cffa7f371ab",
|
"observation_id": "0b3c110f-307c-48f6-9012-656bbc3cd9bf",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 205,
|
"marker_id": 205,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -910,7 +910,7 @@
|
|||||||
"confidence": 0.28724459014346065
|
"confidence": 0.28724459014346065
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "4b7bdee0-6a52-452c-b260-7eea65ad36c2",
|
"observation_id": "698b3a77-1475-47ff-a9e7-c14d8240d428",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 207,
|
"marker_id": 207,
|
||||||
"marker_size_m": 0.025,
|
"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",
|
"schema_version": "1.0",
|
||||||
"created_utc": "2026-05-28T21:28:21Z",
|
"created_utc": "2026-05-28T21:44:43Z",
|
||||||
"vision_config": {
|
"vision_config": {
|
||||||
"MarkerType": "DICT_4X4_250",
|
"MarkerType": "DICT_4X4_250",
|
||||||
"MarkerSize": 0.025
|
"MarkerSize": 0.025
|
||||||
@@ -46,7 +46,7 @@
|
|||||||
},
|
},
|
||||||
"detections": [
|
"detections": [
|
||||||
{
|
{
|
||||||
"observation_id": "0c8f6caa-0447-4f7c-99c4-2d30e31d30d6",
|
"observation_id": "0ebc83d9-bb33-40a7-943b-78666c577574",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 102,
|
"marker_id": 102,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -100,7 +100,7 @@
|
|||||||
"confidence": 0.8913851020933449
|
"confidence": 0.8913851020933449
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "2df4a132-33c5-4ebf-b29f-dcc52478335b",
|
"observation_id": "3e2026ef-788c-4f2c-9d0e-3bb4a4098c49",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 124,
|
"marker_id": 124,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -154,7 +154,7 @@
|
|||||||
"confidence": 0.4804167810936165
|
"confidence": 0.4804167810936165
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "ba6b5ed9-6a18-47e7-a843-af281c19c30f",
|
"observation_id": "ebb20469-1b13-413a-8ea1-a35701ba2bd4",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 243,
|
"marker_id": 243,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -208,7 +208,7 @@
|
|||||||
"confidence": 0.9132502955127223
|
"confidence": 0.9132502955127223
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "f27e2575-2b8b-497b-be0a-b993197fd894",
|
"observation_id": "211ff361-5d5c-4822-88b6-c5cec573090d",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 122,
|
"marker_id": 122,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -262,7 +262,7 @@
|
|||||||
"confidence": 0.9456847621099602
|
"confidence": 0.9456847621099602
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "a2356407-9a0c-460e-a2af-812ffddaddbf",
|
"observation_id": "45c8a29e-1471-4fb7-bffc-27bdeb579be1",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 247,
|
"marker_id": 247,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -316,7 +316,7 @@
|
|||||||
"confidence": 0.7448005120854795
|
"confidence": 0.7448005120854795
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "e37d03aa-9079-465a-8278-e568958327fc",
|
"observation_id": "16d87417-4ffe-4ce0-b51f-1e3fe5c9695e",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 246,
|
"marker_id": 246,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -370,7 +370,7 @@
|
|||||||
"confidence": 0.7581700566520715
|
"confidence": 0.7581700566520715
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "64462f28-959c-436f-9e7a-7b7e2c180bb7",
|
"observation_id": "8c33c157-e141-4212-9279-69ad56760f53",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 215,
|
"marker_id": 215,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -424,7 +424,7 @@
|
|||||||
"confidence": 0.826449608683203
|
"confidence": 0.826449608683203
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "23c3a60a-e454-4ac1-9ba7-13505aa8ffb7",
|
"observation_id": "fe48446c-7538-4bd8-8cec-e96c3e5148b9",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 210,
|
"marker_id": 210,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -478,7 +478,7 @@
|
|||||||
"confidence": 0.7813266383036261
|
"confidence": 0.7813266383036261
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "977958d8-7446-4234-ab37-604bb459f146",
|
"observation_id": "31c12fb9-cb8b-4677-89dc-b24351a14990",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 229,
|
"marker_id": 229,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -532,7 +532,7 @@
|
|||||||
"confidence": 0.18684041285479083
|
"confidence": 0.18684041285479083
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "28ae43e2-3bd3-4829-87d0-e9cb9526e304",
|
"observation_id": "a059901a-85cf-42e8-870f-c88813554c08",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 198,
|
"marker_id": 198,
|
||||||
"marker_size_m": 0.025,
|
"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",
|
"schema_version": "1.0",
|
||||||
"created_utc": "2026-05-28T21:28:21Z",
|
"created_utc": "2026-05-28T21:44:43Z",
|
||||||
"vision_config": {
|
"vision_config": {
|
||||||
"MarkerType": "DICT_4X4_250",
|
"MarkerType": "DICT_4X4_250",
|
||||||
"MarkerSize": 0.025
|
"MarkerSize": 0.025
|
||||||
@@ -46,7 +46,7 @@
|
|||||||
},
|
},
|
||||||
"detections": [
|
"detections": [
|
||||||
{
|
{
|
||||||
"observation_id": "56c7970c-3fdc-437a-82dd-918ef861ac60",
|
"observation_id": "b2febb1e-6f84-4da7-b222-dad6bb306cfc",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 102,
|
"marker_id": 102,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -100,7 +100,7 @@
|
|||||||
"confidence": 0.7012635429952238
|
"confidence": 0.7012635429952238
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "4166196c-2e75-4461-97c1-378b70b0754f",
|
"observation_id": "faabcc45-d069-4b1e-bbf4-a12019902b8a",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 122,
|
"marker_id": 122,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -154,7 +154,7 @@
|
|||||||
"confidence": 0.5749326438029929
|
"confidence": 0.5749326438029929
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "1e515a3c-1477-4fc5-b2e9-4a5c759201dc",
|
"observation_id": "0773bfb1-9c4f-4add-9a74-bca81c38b2e8",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 243,
|
"marker_id": 243,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -208,7 +208,7 @@
|
|||||||
"confidence": 0.9491420540234864
|
"confidence": 0.9491420540234864
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "1549938d-da8e-4883-9239-92b88fc508c9",
|
"observation_id": "abf720ab-c1d5-40c7-ad4f-9f58f8bd6ee1",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 246,
|
"marker_id": 246,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -262,7 +262,7 @@
|
|||||||
"confidence": 0.4780463267147134
|
"confidence": 0.4780463267147134
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "634d2ef2-2e64-4009-8d92-c16368d1b20a",
|
"observation_id": "13752012-4478-43b7-b2f9-eab237d61864",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 247,
|
"marker_id": 247,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -316,7 +316,7 @@
|
|||||||
"confidence": 0.5106218488640142
|
"confidence": 0.5106218488640142
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "e96b7926-73c9-42c3-9c6e-e7bd66a0b4b7",
|
"observation_id": "823b9adb-31fb-414d-aaaa-552432206d5c",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 214,
|
"marker_id": 214,
|
||||||
"marker_size_m": 0.025,
|
"marker_size_m": 0.025,
|
||||||
@@ -370,7 +370,7 @@
|
|||||||
"confidence": 0.6041252446922665
|
"confidence": 0.6041252446922665
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"observation_id": "c57683ff-d5c4-412c-b893-95d36298186f",
|
"observation_id": "a33bdbe4-4e7d-42da-9c9e-6b5eaf4b5d8f",
|
||||||
"type": "aruco",
|
"type": "aruco",
|
||||||
"marker_id": 210,
|
"marker_id": 210,
|
||||||
"marker_size_m": 0.025,
|
"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_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_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_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": {
|
"renderingInfo": {
|
||||||
"width": 1280,
|
"width": 1280,
|
||||||
"height": 720,
|
"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],
|
"cameraTarget": [210, -180, 180],
|
||||||
"cameraUpVector": [0, 0, 1],
|
"cameraUpVector": [0, 0, 1],
|
||||||
"lightPosition": [-500, -500, 500],
|
"lightPosition": [-500, -500, 500],
|
||||||
@@ -78,10 +84,19 @@
|
|||||||
}
|
}
|
||||||
},
|
},
|
||||||
"defaultPosition": {
|
"defaultPosition": {
|
||||||
"x": 100,
|
"x": 10,
|
||||||
"y": 30,
|
"y": 4,
|
||||||
"z": -30,
|
"z": 20,
|
||||||
"a": -120,
|
"a": 10,
|
||||||
|
"b": 2,
|
||||||
|
"c": 9,
|
||||||
|
"e": 1
|
||||||
|
},
|
||||||
|
"defaultPosition____": {
|
||||||
|
"x": 140,
|
||||||
|
"y": 46,
|
||||||
|
"z": -70,
|
||||||
|
"a": 120,
|
||||||
"b": 22,
|
"b": 22,
|
||||||
"c": 91,
|
"c": 91,
|
||||||
"e": 10
|
"e": 10
|
||||||
@@ -95,22 +110,22 @@
|
|||||||
"c": null,
|
"c": null,
|
||||||
"e": null
|
"e": null
|
||||||
},
|
},
|
||||||
"multiview_quality": {
|
"multiview_calculation": {
|
||||||
"combine_mode": "mean",
|
"combine_mode": "mean",
|
||||||
"size_ref_px": 50.0,
|
"size_ref_px": 50.0,
|
||||||
"border_ref_px": 120.0,
|
"border_ref_px": 120.0,
|
||||||
"center_ref_norm": 0.01,
|
"center_ref_norm": 0.01,
|
||||||
"sharpness_ref": 2500.0,
|
"sharpness_ref": 2500.0,
|
||||||
"homography_ref": 0.18,
|
"homography_ref": 0.18,
|
||||||
"size_factor": 0.01,
|
"size_factor": 0.3,
|
||||||
"aspect_factor": 0.01,
|
"aspect_factor": 0.3,
|
||||||
"border_factor": 0.01,
|
"border_factor": 0.01,
|
||||||
"center_factor": 0.01,
|
"center_factor": 0.01,
|
||||||
"sharpness_factor": 0.01,
|
"sharpness_factor": 0.5,
|
||||||
"homography_factor": 0.01,
|
"homography_factor": 0.2,
|
||||||
"normal_visibility_factor": 0.01,
|
"normal_visibility_factor": 0.01,
|
||||||
"spin_factor": 0.01,
|
"spin_factor": 0.3,
|
||||||
"weight_floor": 0.01
|
"weight_floor": 0.3
|
||||||
},
|
},
|
||||||
"movements": {
|
"movements": {
|
||||||
"x": null,
|
"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