iniciado testes com depth
This commit is contained in:
Diego Freitas
parent
4ec3c35759
commit
7581989647
|
|
@ -0,0 +1,247 @@
|
|||
{
|
||||
"batchName": "",
|
||||
"batchTime": 1756192307,
|
||||
"boardConf": "nIR-C00M05-00",
|
||||
"boardCustom": "",
|
||||
"boardName": "DM1090",
|
||||
"boardOptions": 0,
|
||||
"boardRev": "R3M0E3",
|
||||
"cameraData": [
|
||||
[
|
||||
1,
|
||||
{
|
||||
"cameraType": 0,
|
||||
"distortionCoeff": [
|
||||
-0.2667059302330017,
|
||||
0.24688485264778137,
|
||||
-0.000222075788769871,
|
||||
0.00012030570360366255,
|
||||
-0.039670947939157486,
|
||||
-0.26931697130203247,
|
||||
0.24513530731201172,
|
||||
-0.039285946637392044,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"extrinsics": {
|
||||
"rotationMatrix": [
|
||||
[
|
||||
0.9999990463256836,
|
||||
-0.00016664451686665416,
|
||||
-0.0013813182013109326
|
||||
],
|
||||
[
|
||||
0.00011326302046654746,
|
||||
0.999256432056427,
|
||||
-0.038555704057216644
|
||||
],
|
||||
[
|
||||
0.0013867162633687258,
|
||||
0.038555510342121124,
|
||||
0.9992554783821106
|
||||
]
|
||||
],
|
||||
"specTranslation": {
|
||||
"x": 2.5,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
},
|
||||
"toCameraSocket": 2,
|
||||
"translation": {
|
||||
"x": 2.3664393424987793,
|
||||
"y": 0.2041260451078415,
|
||||
"z": 8.34726333618164
|
||||
}
|
||||
},
|
||||
"height": 800,
|
||||
"intrinsicMatrix": [
|
||||
[
|
||||
329.8727111816406,
|
||||
0.0,
|
||||
655.0603637695312
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
329.14971923828125,
|
||||
353.3602600097656
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"lensPosition": 0,
|
||||
"specHfovDeg": 70.9000015258789,
|
||||
"width": 1280
|
||||
}
|
||||
],
|
||||
[
|
||||
2,
|
||||
{
|
||||
"cameraType": 0,
|
||||
"distortionCoeff": [
|
||||
2.599353075027466,
|
||||
1.7201688289642334,
|
||||
-0.0002882175613194704,
|
||||
0.0005083332071080804,
|
||||
-2.265909433364868,
|
||||
2.5841195583343506,
|
||||
1.6573377847671509,
|
||||
-2.2209253311157227,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"extrinsics": {
|
||||
"rotationMatrix": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"specTranslation": {
|
||||
"x": -0.0,
|
||||
"y": -0.0,
|
||||
"z": -0.0
|
||||
},
|
||||
"toCameraSocket": -1,
|
||||
"translation": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
}
|
||||
},
|
||||
"height": 800,
|
||||
"intrinsicMatrix": [
|
||||
[
|
||||
577.9281005859375,
|
||||
0.0,
|
||||
644.1484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
577.5459594726562,
|
||||
379.2917175292969
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"lensPosition": 0,
|
||||
"specHfovDeg": 70.9000015258789,
|
||||
"width": 1280
|
||||
}
|
||||
]
|
||||
],
|
||||
"deviceName": "",
|
||||
"hardwareConf": "F0-FV00-BC000",
|
||||
"housingExtrinsics": {
|
||||
"rotationMatrix": [],
|
||||
"specTranslation": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
},
|
||||
"toCameraSocket": -1,
|
||||
"translation": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
}
|
||||
},
|
||||
"imuExtrinsics": {
|
||||
"rotationMatrix": [
|
||||
[
|
||||
1.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
1.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"specTranslation": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
},
|
||||
"toCameraSocket": -1,
|
||||
"translation": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"z": 0.0
|
||||
}
|
||||
},
|
||||
"miscellaneousData": [],
|
||||
"productName": "OAK-FFC-3P",
|
||||
"stereoEnableDistortionCorrection": false,
|
||||
"stereoRectificationData": {
|
||||
"leftCameraSocket": 1,
|
||||
"rectifiedRotationLeft": [
|
||||
[
|
||||
0.27401065826416016,
|
||||
0.06054104119539261,
|
||||
0.9598191976547241
|
||||
],
|
||||
[
|
||||
-0.0419992059469223,
|
||||
0.9978178143501282,
|
||||
-0.050947822630405426
|
||||
],
|
||||
[
|
||||
-0.9608091711997986,
|
||||
-0.02635139599442482,
|
||||
0.2759554088115692
|
||||
]
|
||||
],
|
||||
"rectifiedRotationRight": [
|
||||
[
|
||||
0.2726745009422302,
|
||||
0.02352055534720421,
|
||||
0.9618188142776489
|
||||
],
|
||||
[
|
||||
-0.04209506884217262,
|
||||
0.9990354776382446,
|
||||
-0.012496757321059704
|
||||
],
|
||||
[
|
||||
-0.9611850380897522,
|
||||
-0.03708028048276901,
|
||||
0.2734015882015228
|
||||
]
|
||||
],
|
||||
"rightCameraSocket": 2
|
||||
},
|
||||
"stereoUseSpecTranslation": true,
|
||||
"version": 7,
|
||||
"verticalCameraSocket": -1
|
||||
}
|
||||
|
|
@ -0,0 +1,282 @@
|
|||
{
|
||||
"created_at": "2026-05-25 13:57:51",
|
||||
"device_id": "194430108133AC2F00",
|
||||
"calibration_read_method": "readCalibration",
|
||||
"connected_cameras": [
|
||||
{
|
||||
"socket": "CAM_A",
|
||||
"sensorName": "OV9782",
|
||||
"width": 1280,
|
||||
"height": 800,
|
||||
"orientation": "CameraImageOrientation.AUTO",
|
||||
"supportedTypes": [
|
||||
"CameraSensorType.COLOR",
|
||||
"CameraSensorType.MONO"
|
||||
]
|
||||
},
|
||||
{
|
||||
"socket": "CAM_B",
|
||||
"sensorName": "OV9282",
|
||||
"width": 1280,
|
||||
"height": 800,
|
||||
"orientation": "CameraImageOrientation.AUTO",
|
||||
"supportedTypes": [
|
||||
"CameraSensorType.MONO",
|
||||
"CameraSensorType.COLOR"
|
||||
]
|
||||
},
|
||||
{
|
||||
"socket": "CAM_C",
|
||||
"sensorName": "OV9282",
|
||||
"width": 1280,
|
||||
"height": 800,
|
||||
"orientation": "CameraImageOrientation.AUTO",
|
||||
"supportedTypes": [
|
||||
"CameraSensorType.MONO",
|
||||
"CameraSensorType.COLOR"
|
||||
]
|
||||
}
|
||||
],
|
||||
"sockets_requested": [
|
||||
"CAM_A",
|
||||
"CAM_B",
|
||||
"CAM_C"
|
||||
],
|
||||
"width": 1280,
|
||||
"height": 800,
|
||||
"calibration_dump": {
|
||||
"available": true,
|
||||
"method": "eepromToJson",
|
||||
"path": "calibration\\calibration_probe_out\\calibration_eepromToJson.json",
|
||||
"error": null
|
||||
},
|
||||
"sockets": [
|
||||
{
|
||||
"socket": "CAM_A",
|
||||
"intrinsics": null,
|
||||
"intrinsics_method": "failed:getCameraIntrinsics",
|
||||
"intrinsics_ok": false,
|
||||
"distortion": null,
|
||||
"distortion_method": "missing:distortion",
|
||||
"distortion_ok": false,
|
||||
"fov_deg": null,
|
||||
"fov_method": "missing:getFov"
|
||||
},
|
||||
{
|
||||
"socket": "CAM_B",
|
||||
"intrinsics": [
|
||||
[
|
||||
329.8727111816406,
|
||||
0.0,
|
||||
655.0603637695312
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
329.14971923828125,
|
||||
353.3602600097656
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"intrinsics_method": "getCameraIntrinsics3args",
|
||||
"intrinsics_ok": true,
|
||||
"distortion": [
|
||||
-0.2667059302330017,
|
||||
0.24688485264778137,
|
||||
-0.000222075788769871,
|
||||
0.00012030570360366255,
|
||||
-0.039670947939157486,
|
||||
-0.26931697130203247,
|
||||
0.24513530731201172,
|
||||
-0.039285946637392044,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"distortion_method": "getDistortionCoefficients",
|
||||
"distortion_ok": true,
|
||||
"fov_deg": 70.9000015258789,
|
||||
"fov_method": "getFov"
|
||||
},
|
||||
{
|
||||
"socket": "CAM_C",
|
||||
"intrinsics": [
|
||||
[
|
||||
577.9281005859375,
|
||||
0.0,
|
||||
644.1484375
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
577.5459594726562,
|
||||
379.2917175292969
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"intrinsics_method": "getCameraIntrinsics3args",
|
||||
"intrinsics_ok": true,
|
||||
"distortion": [
|
||||
2.599353075027466,
|
||||
1.7201688289642334,
|
||||
-0.0002882175613194704,
|
||||
0.0005083332071080804,
|
||||
-2.265909433364868,
|
||||
2.5841195583343506,
|
||||
1.6573377847671509,
|
||||
-2.2209253311157227,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"distortion_method": "getDistortionCoefficients",
|
||||
"distortion_ok": true,
|
||||
"fov_deg": 70.9000015258789,
|
||||
"fov_method": "getFov"
|
||||
}
|
||||
],
|
||||
"pairs": [
|
||||
{
|
||||
"pair": "CAM_A->CAM_B",
|
||||
"src": "CAM_A",
|
||||
"dst": "CAM_B",
|
||||
"extrinsics": null,
|
||||
"extrinsics_method": "failed:getCameraExtrinsics",
|
||||
"extrinsics_ok": false,
|
||||
"translation": null,
|
||||
"baseline": null,
|
||||
"baseline_method": "missing:getBaselineDistance",
|
||||
"baseline_ok": false
|
||||
},
|
||||
{
|
||||
"pair": "CAM_A->CAM_C",
|
||||
"src": "CAM_A",
|
||||
"dst": "CAM_C",
|
||||
"extrinsics": null,
|
||||
"extrinsics_method": "failed:getCameraExtrinsics",
|
||||
"extrinsics_ok": false,
|
||||
"translation": null,
|
||||
"baseline": null,
|
||||
"baseline_method": "missing:getBaselineDistance",
|
||||
"baseline_ok": false
|
||||
},
|
||||
{
|
||||
"pair": "CAM_B->CAM_A",
|
||||
"src": "CAM_B",
|
||||
"dst": "CAM_A",
|
||||
"extrinsics": null,
|
||||
"extrinsics_method": "failed:getCameraExtrinsics",
|
||||
"extrinsics_ok": false,
|
||||
"translation": null,
|
||||
"baseline": null,
|
||||
"baseline_method": "missing:getBaselineDistance",
|
||||
"baseline_ok": false
|
||||
},
|
||||
{
|
||||
"pair": "CAM_B->CAM_C",
|
||||
"src": "CAM_B",
|
||||
"dst": "CAM_C",
|
||||
"extrinsics": [
|
||||
[
|
||||
0.9999990463256836,
|
||||
-0.00016664451686665416,
|
||||
-0.0013813182013109326,
|
||||
2.3664393424987793
|
||||
],
|
||||
[
|
||||
0.00011326302046654746,
|
||||
0.999256432056427,
|
||||
-0.038555704057216644,
|
||||
0.2041260451078415
|
||||
],
|
||||
[
|
||||
0.0013867162633687258,
|
||||
0.038555510342121124,
|
||||
0.9992554783821106,
|
||||
8.34726333618164
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"extrinsics_method": "getCameraExtrinsics2args",
|
||||
"extrinsics_ok": true,
|
||||
"translation": [
|
||||
2.3664393424987793,
|
||||
0.2041260451078415,
|
||||
8.34726333618164
|
||||
],
|
||||
"baseline": 2.5,
|
||||
"baseline_method": "getBaselineDistance2args",
|
||||
"baseline_ok": true
|
||||
},
|
||||
{
|
||||
"pair": "CAM_C->CAM_A",
|
||||
"src": "CAM_C",
|
||||
"dst": "CAM_A",
|
||||
"extrinsics": null,
|
||||
"extrinsics_method": "failed:getCameraExtrinsics",
|
||||
"extrinsics_ok": false,
|
||||
"translation": null,
|
||||
"baseline": null,
|
||||
"baseline_method": "missing:getBaselineDistance",
|
||||
"baseline_ok": false
|
||||
},
|
||||
{
|
||||
"pair": "CAM_C->CAM_B",
|
||||
"src": "CAM_C",
|
||||
"dst": "CAM_B",
|
||||
"extrinsics": [
|
||||
[
|
||||
0.9999990463256836,
|
||||
0.00011326302046654746,
|
||||
0.0013867162633687258,
|
||||
-2.378035545349121
|
||||
],
|
||||
[
|
||||
-0.00016664451686665416,
|
||||
0.999256432056427,
|
||||
0.038555510342121124,
|
||||
-0.525412917137146
|
||||
],
|
||||
[
|
||||
-0.0013813182013109326,
|
||||
-0.038555704057216644,
|
||||
0.9992554783821106,
|
||||
-8.329909324645996
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"extrinsics_method": "getCameraExtrinsics2args",
|
||||
"extrinsics_ok": true,
|
||||
"translation": [
|
||||
-2.378035545349121,
|
||||
-0.525412917137146,
|
||||
-8.329909324645996
|
||||
],
|
||||
"baseline": 2.5,
|
||||
"baseline_method": "getBaselineDistance2args",
|
||||
"baseline_ok": true
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -588,15 +588,15 @@
|
|||
"reference_mode": "fixed",
|
||||
"reference_controls": {
|
||||
"rgb": {
|
||||
"exposure_time_us": 2200,
|
||||
"exposure_time_us": 3000,
|
||||
"sensitivity_iso": 100
|
||||
},
|
||||
"re": {
|
||||
"exposure_time_us": 2500,
|
||||
"exposure_time_us": 4000,
|
||||
"sensitivity_iso": 100
|
||||
},
|
||||
"nir": {
|
||||
"exposure_time_us": 2500,
|
||||
"exposure_time_us": 4000,
|
||||
"sensitivity_iso": 100
|
||||
}
|
||||
},
|
||||
|
|
@ -612,11 +612,11 @@
|
|||
},
|
||||
"re": {
|
||||
"min": 0.15,
|
||||
"max": 2.5
|
||||
"max": 3.0
|
||||
},
|
||||
"nir": {
|
||||
"min": 0.15,
|
||||
"max": 2.5
|
||||
"max": 3.0
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -679,7 +679,7 @@
|
|||
"rgb_saturation_threshold": 0.97
|
||||
},
|
||||
"rgb_calibration": {
|
||||
"enabled": true,
|
||||
"enabled": false,
|
||||
"gains": {
|
||||
"R": 1.061,
|
||||
"G": 1.0,
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
{
|
||||
"camera": "oak-fcc-3",
|
||||
"modelo": "segformer_b1",
|
||||
"model_name": "target_aug",
|
||||
"model_name": "target_fixed",
|
||||
"main_class_name": "cana",
|
||||
"es_classes": "",
|
||||
"model_to_use": "geral",
|
||||
|
|
|
|||
|
|
@ -878,6 +878,19 @@ class OakFcc3Manager:
|
|||
self.has_imu_pipeline = False
|
||||
self.running = False
|
||||
|
||||
def _is_fatal_depthai_error(self, erro):
|
||||
txt = str(erro)
|
||||
|
||||
sinais = [
|
||||
"X_LINK_ERROR",
|
||||
"Communication exception",
|
||||
"Couldn't read data from stream",
|
||||
"Device already closed",
|
||||
"device has been closed",
|
||||
]
|
||||
|
||||
return any(s in txt for s in sinais)
|
||||
|
||||
# ============================================================
|
||||
# Status
|
||||
# ============================================================
|
||||
|
|
@ -924,7 +937,15 @@ class OakFcc3Manager:
|
|||
|
||||
def get_next_frame(self, timeout=1.0):
|
||||
if not self.running:
|
||||
raise RuntimeError("OakFcc3Manager não está rodando. Chame start() primeiro.")
|
||||
last_error = None
|
||||
try:
|
||||
last_error = self._capture_thread_stats.get("last_error")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
raise RuntimeError(
|
||||
f"OakFcc3Manager não está rodando. Último erro: {last_error}"
|
||||
)
|
||||
|
||||
# Fallback síncrono se desligar async.
|
||||
if not bool(getattr(self, "async_capture_enabled", True)):
|
||||
|
|
@ -1785,10 +1806,34 @@ class OakFcc3Manager:
|
|||
self._capture_cond.notify_all()
|
||||
|
||||
except Exception as e:
|
||||
erro = f"{type(e).__name__}: {e}"
|
||||
|
||||
try:
|
||||
self._capture_thread_stats["last_error"] = f"{type(e).__name__}: {e}"
|
||||
self._capture_thread_stats["last_error"] = erro
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if self._is_fatal_depthai_error(e):
|
||||
try:
|
||||
self._capture_thread_stats["fatal_error"] = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
self.running = False
|
||||
|
||||
try:
|
||||
self._capture_stop_event.set()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
with self._capture_cond:
|
||||
self._capture_cond.notify_all()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
break
|
||||
|
||||
time.sleep(0.005)
|
||||
|
||||
try:
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,420 @@
|
|||
import argparse
|
||||
import time
|
||||
from collections import deque
|
||||
from typing import Optional
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import depthai as dai
|
||||
except Exception as e:
|
||||
raise RuntimeError(
|
||||
"Nao consegui importar depthai. Ative o venv correto e instale depthai antes de rodar. "
|
||||
f"Erro original: {e}"
|
||||
)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# OAK-FCC-3P Charuco Preview Probe
|
||||
# ------------------------------------------------------------
|
||||
# Objetivo:
|
||||
# Abrir CAM_A/CAM_B/CAM_C ao vivo para verificar se o Charuco no monitor
|
||||
# ou impresso aparece bem nas tres cameras, principalmente nas mono RE/NIR.
|
||||
#
|
||||
# Exemplo:
|
||||
# python -m utils.charuco_preview_probe --fps 10
|
||||
#
|
||||
# Teclas:
|
||||
# Q/ESC = sair
|
||||
# S = salvar snapshot
|
||||
# E = alterna detector de bordas
|
||||
# C = alterna contraste auto/normal nas mono
|
||||
# ============================================================
|
||||
|
||||
|
||||
# ============================================================
|
||||
# DepthAI helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def socket_from_name(name: str):
|
||||
name = str(name).strip().upper()
|
||||
aliases = {
|
||||
"A": "CAM_A",
|
||||
"B": "CAM_B",
|
||||
"C": "CAM_C",
|
||||
"RGB": "CAM_A",
|
||||
"RE": "CAM_B",
|
||||
"NIR": "CAM_C",
|
||||
}
|
||||
name = aliases.get(name, name)
|
||||
|
||||
if hasattr(dai.CameraBoardSocket, name):
|
||||
return getattr(dai.CameraBoardSocket, name)
|
||||
|
||||
legacy = {
|
||||
"CAM_A": getattr(dai.CameraBoardSocket, "RGB", None),
|
||||
"CAM_B": getattr(dai.CameraBoardSocket, "LEFT", None),
|
||||
"CAM_C": getattr(dai.CameraBoardSocket, "RIGHT", None),
|
||||
}
|
||||
if legacy.get(name) is not None:
|
||||
return legacy[name]
|
||||
|
||||
raise ValueError(f"Socket invalido: {name}. Use CAM_A, CAM_B ou CAM_C.")
|
||||
|
||||
|
||||
|
||||
def mono_resolution_from_name(name: str):
|
||||
name = str(name).strip().lower()
|
||||
r = dai.MonoCameraProperties.SensorResolution
|
||||
table = {
|
||||
"400p": getattr(r, "THE_400_P", None),
|
||||
"480p": getattr(r, "THE_480_P", None),
|
||||
"720p": getattr(r, "THE_720_P", None),
|
||||
"800p": getattr(r, "THE_800_P", None),
|
||||
}
|
||||
if name not in table or table[name] is None:
|
||||
valid = ", ".join(k for k, v in table.items() if v is not None)
|
||||
raise ValueError(f"Resolucao mono invalida: {name}. Valid={valid}")
|
||||
return table[name]
|
||||
|
||||
|
||||
|
||||
def create_output_queue(output, name: str, max_size: int = 4, blocking: bool = False):
|
||||
fn = getattr(output, "createOutputQueue", None)
|
||||
if callable(fn):
|
||||
return fn(maxSize=max_size, blocking=blocking)
|
||||
raise RuntimeError(f"A saida '{name}' nao possui createOutputQueue().")
|
||||
|
||||
|
||||
|
||||
def get_frame(q) -> Optional[np.ndarray]:
|
||||
if q is None:
|
||||
return None
|
||||
try:
|
||||
msg = q.tryGet()
|
||||
except Exception:
|
||||
return None
|
||||
if msg is None:
|
||||
return None
|
||||
|
||||
try:
|
||||
return msg.getCvFrame()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
return msg.getFrame()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Visual helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def normalize_u8(img: np.ndarray, auto: bool = True) -> np.ndarray:
|
||||
if img is None:
|
||||
return np.zeros((300, 400), dtype=np.uint8)
|
||||
|
||||
arr = np.asarray(img)
|
||||
if arr.ndim == 3:
|
||||
return arr.astype(np.uint8)
|
||||
|
||||
arr = arr.astype(np.float32)
|
||||
if not auto:
|
||||
if arr.max() <= 1.5:
|
||||
return np.clip(arr * 255.0, 0, 255).astype(np.uint8)
|
||||
return np.clip(arr, 0, 255).astype(np.uint8)
|
||||
|
||||
finite = np.isfinite(arr)
|
||||
if not np.any(finite):
|
||||
return np.zeros(arr.shape[:2], dtype=np.uint8)
|
||||
vals = arr[finite]
|
||||
lo = float(np.percentile(vals, 1))
|
||||
hi = float(np.percentile(vals, 99))
|
||||
if hi <= lo + 1e-6:
|
||||
hi = lo + 1.0
|
||||
out = np.clip((arr - lo) / (hi - lo), 0, 1)
|
||||
return (out * 255).astype(np.uint8)
|
||||
|
||||
|
||||
|
||||
def edge_view(gray_u8: np.ndarray) -> np.ndarray:
|
||||
if gray_u8.ndim == 3:
|
||||
gray_u8 = cv2.cvtColor(gray_u8, cv2.COLOR_BGR2GRAY)
|
||||
edges = cv2.Canny(gray_u8, 60, 140)
|
||||
return edges
|
||||
|
||||
|
||||
|
||||
def put_label(img: np.ndarray, title: str, subtitle: str = "") -> np.ndarray:
|
||||
if img.ndim == 2:
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
out = img.copy()
|
||||
hbox = 58 if subtitle else 36
|
||||
cv2.rectangle(out, (0, 0), (out.shape[1], hbox), (0, 0, 0), -1)
|
||||
cv2.putText(out, str(title)[:80], (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2, cv2.LINE_AA)
|
||||
if subtitle:
|
||||
cv2.putText(out, str(subtitle)[:115], (10, 48), cv2.FONT_HERSHEY_SIMPLEX, 0.43, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
return out
|
||||
|
||||
|
||||
|
||||
def resize_keep(img: np.ndarray, width: int) -> np.ndarray:
|
||||
scale = width / img.shape[1]
|
||||
height = max(1, int(img.shape[0] * scale))
|
||||
return cv2.resize(img, (width, height), interpolation=cv2.INTER_AREA)
|
||||
|
||||
|
||||
|
||||
def make_grid(panels, panel_w: int = 520, cols: int = 3) -> np.ndarray:
|
||||
rendered = []
|
||||
for title, img, subtitle in panels:
|
||||
if img.ndim == 2:
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
small = resize_keep(img, panel_w)
|
||||
rendered.append(put_label(small, title, subtitle))
|
||||
|
||||
max_h = max(x.shape[0] for x in rendered)
|
||||
padded = []
|
||||
for im in rendered:
|
||||
if im.shape[0] < max_h:
|
||||
pad = np.zeros((max_h - im.shape[0], im.shape[1], 3), dtype=np.uint8)
|
||||
im = np.vstack([im, pad])
|
||||
padded.append(im)
|
||||
|
||||
gap = 10
|
||||
gap_w = np.full((max_h, gap, 3), 25, dtype=np.uint8)
|
||||
rows = []
|
||||
for i in range(0, len(padded), cols):
|
||||
items = padded[i:i + cols]
|
||||
while len(items) < cols:
|
||||
items.append(np.zeros_like(padded[0]))
|
||||
row = items[0]
|
||||
for j in range(1, cols):
|
||||
row = np.hstack([row, gap_w, items[j]])
|
||||
rows.append(row)
|
||||
|
||||
gap_h = np.full((gap, rows[0].shape[1], 3), 25, dtype=np.uint8)
|
||||
canvas = rows[0]
|
||||
for row in rows[1:]:
|
||||
canvas = np.vstack([canvas, gap_h, row])
|
||||
return canvas
|
||||
|
||||
|
||||
|
||||
def stats_line(img: np.ndarray) -> str:
|
||||
if img is None:
|
||||
return "sem frame"
|
||||
arr = np.asarray(img, dtype=np.float32)
|
||||
if arr.ndim == 3:
|
||||
gray = cv2.cvtColor(arr.astype(np.uint8), cv2.COLOR_BGR2GRAY).astype(np.float32)
|
||||
else:
|
||||
gray = arr
|
||||
return f"mean={gray.mean():.1f} p05={np.percentile(gray,5):.1f} p95={np.percentile(gray,95):.1f}"
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Pipeline
|
||||
# ============================================================
|
||||
|
||||
|
||||
def create_pipeline_and_outputs(args):
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
# CAM_A color
|
||||
rgb = pipeline.create(dai.node.ColorCamera)
|
||||
rgb.setBoardSocket(socket_from_name(args.rgb))
|
||||
rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
|
||||
rgb.setFps(float(args.fps))
|
||||
rgb.setInterleaved(False)
|
||||
rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
rgb.setPreviewSize(int(args.preview_w), int(args.preview_h))
|
||||
|
||||
# CAM_B/C mono
|
||||
mono_b = pipeline.create(dai.node.MonoCamera)
|
||||
mono_c = pipeline.create(dai.node.MonoCamera)
|
||||
mono_b.setBoardSocket(socket_from_name(args.cam_b))
|
||||
mono_c.setBoardSocket(socket_from_name(args.cam_c))
|
||||
mono_b.setResolution(mono_resolution_from_name(args.mono_resolution))
|
||||
mono_c.setResolution(mono_resolution_from_name(args.mono_resolution))
|
||||
mono_b.setFps(float(args.fps))
|
||||
mono_c.setFps(float(args.fps))
|
||||
|
||||
outputs = {
|
||||
"rgb": rgb.preview,
|
||||
"cam_b": mono_b.out,
|
||||
"cam_c": mono_c.out,
|
||||
}
|
||||
return pipeline, outputs
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Main
|
||||
# ============================================================
|
||||
|
||||
|
||||
def start_pipeline(pipeline):
|
||||
fn = getattr(pipeline, "start", None)
|
||||
if not callable(fn):
|
||||
raise RuntimeError("pipeline.start() nao existe nesta versao do DepthAI.")
|
||||
fn()
|
||||
|
||||
|
||||
|
||||
def stop_pipeline(pipeline):
|
||||
try:
|
||||
fn = getattr(pipeline, "stop", None)
|
||||
if callable(fn):
|
||||
fn()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
|
||||
def save_snapshot(out_dir: str, rgb, cam_b, cam_c, canvas):
|
||||
folder = Path(out_dir)
|
||||
folder.mkdir(parents=True, exist_ok=True)
|
||||
ts = time.strftime("%Y%m%d_%H%M%S")
|
||||
if rgb is not None:
|
||||
cv2.imwrite(str(folder / f"{ts}_CAM_A_rgb.png"), rgb)
|
||||
if cam_b is not None:
|
||||
cv2.imwrite(str(folder / f"{ts}_CAM_B_mono.png"), normalize_u8(cam_b, auto=True))
|
||||
if cam_c is not None:
|
||||
cv2.imwrite(str(folder / f"{ts}_CAM_C_mono.png"), normalize_u8(cam_c, auto=True))
|
||||
if canvas is not None:
|
||||
cv2.imwrite(str(folder / f"{ts}_canvas.png"), canvas)
|
||||
print(f"[OK] snapshot salvo em {folder}")
|
||||
|
||||
|
||||
|
||||
def main(args):
|
||||
pipeline, outputs = create_pipeline_and_outputs(args)
|
||||
|
||||
queues = {
|
||||
name: create_output_queue(output, name, max_size=4, blocking=False)
|
||||
for name, output in outputs.items()
|
||||
}
|
||||
|
||||
print("[INFO] Pipeline preview criado sem StereoDepth.")
|
||||
print("[INFO] Abra o PDF Charuco em tela cheia no monitor e aponte a camera para ele.")
|
||||
print("[INFO] O objetivo e ver se CAM_B e CAM_C enxergam marcadores/cantos com contraste.")
|
||||
|
||||
start_pipeline(pipeline)
|
||||
|
||||
cv2.namedWindow("OAK-FCC-3P Charuco Preview Probe", cv2.WINDOW_NORMAL)
|
||||
cv2.resizeWindow("OAK-FCC-3P Charuco Preview Probe", 1600, 900)
|
||||
|
||||
show_edges = False
|
||||
auto_contrast = True
|
||||
frame_times = deque(maxlen=40)
|
||||
last_canvas = None
|
||||
|
||||
rgb_frame = None
|
||||
b_frame = None
|
||||
c_frame = None
|
||||
|
||||
try:
|
||||
while True:
|
||||
updated = False
|
||||
for name, q in queues.items():
|
||||
frame = get_frame(q)
|
||||
if frame is None:
|
||||
continue
|
||||
updated = True
|
||||
if name == "rgb":
|
||||
rgb_frame = frame
|
||||
elif name == "cam_b":
|
||||
b_frame = frame
|
||||
elif name == "cam_c":
|
||||
c_frame = frame
|
||||
|
||||
if updated:
|
||||
frame_times.append(time.time())
|
||||
|
||||
if len(frame_times) >= 2:
|
||||
fps = (len(frame_times) - 1) / max(1e-6, frame_times[-1] - frame_times[0])
|
||||
else:
|
||||
fps = 0.0
|
||||
|
||||
if rgb_frame is None or b_frame is None or c_frame is None:
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key in (27, ord("q"), ord("Q")):
|
||||
break
|
||||
continue
|
||||
|
||||
rgb_vis = rgb_frame.copy()
|
||||
b_vis = normalize_u8(b_frame, auto=auto_contrast)
|
||||
c_vis = normalize_u8(c_frame, auto=auto_contrast)
|
||||
|
||||
if show_edges:
|
||||
rgb_gray = cv2.cvtColor(rgb_vis, cv2.COLOR_BGR2GRAY)
|
||||
rgb_panel = edge_view(rgb_gray)
|
||||
b_panel = edge_view(b_vis)
|
||||
c_panel = edge_view(c_vis)
|
||||
mode = "edges"
|
||||
else:
|
||||
rgb_panel = rgb_vis
|
||||
b_panel = b_vis
|
||||
c_panel = c_vis
|
||||
mode = "preview"
|
||||
|
||||
panels = [
|
||||
("CAM_A RGB", rgb_panel, f"{stats_line(rgb_frame)} | fps={fps:.1f}"),
|
||||
("CAM_B mono / RE", b_panel, stats_line(b_frame)),
|
||||
("CAM_C mono / NIR", c_panel, stats_line(c_frame)),
|
||||
("CAM_B edges" if not show_edges else "CAM_B preview", edge_view(b_vis) if not show_edges else b_vis, "bordas para ver marcador"),
|
||||
("CAM_C edges" if not show_edges else "CAM_C preview", edge_view(c_vis) if not show_edges else c_vis, "bordas para ver marcador"),
|
||||
("Info", np.zeros((300, 600, 3), dtype=np.uint8), f"mode={mode} auto_contrast={auto_contrast} | E edges | C contraste | S save | Q sair"),
|
||||
]
|
||||
|
||||
canvas = make_grid(panels, panel_w=args.panel_w, cols=3)
|
||||
|
||||
# Escreve texto grande no painel Info vazio, ultimo quadrante.
|
||||
info_y0 = canvas.shape[0] - resize_keep(np.zeros((300, 600, 3), dtype=np.uint8), args.panel_w).shape[0]
|
||||
cv2.putText(canvas, "Charuco visibility test", (2 * (args.panel_w + 10) + 15, info_y0 + 95), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 255, 255), 2, cv2.LINE_AA)
|
||||
cv2.putText(canvas, "Olhe CAM_B/C: marcadores precisam aparecer nitidos", (2 * (args.panel_w + 10) + 15, info_y0 + 135), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
cv2.putText(canvas, "E=edges C=auto contrast S=snapshot Q=sair", (2 * (args.panel_w + 10) + 15, info_y0 + 170), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
|
||||
last_canvas = canvas
|
||||
cv2.imshow("OAK-FCC-3P Charuco Preview Probe", canvas)
|
||||
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key in (27, ord("q"), ord("Q")):
|
||||
break
|
||||
elif key in (ord("e"), ord("E")):
|
||||
show_edges = not show_edges
|
||||
elif key in (ord("c"), ord("C")):
|
||||
auto_contrast = not auto_contrast
|
||||
elif key in (ord("s"), ord("S")):
|
||||
save_snapshot(args.out_dir, rgb_frame, b_frame, c_frame, last_canvas)
|
||||
|
||||
finally:
|
||||
stop_pipeline(pipeline)
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
# ============================================================
|
||||
# CLI
|
||||
# ============================================================
|
||||
|
||||
|
||||
def build_argparser():
|
||||
ap = argparse.ArgumentParser(description="Preview rapido CAM_A/CAM_B/CAM_C para testar visibilidade do Charuco.")
|
||||
ap.add_argument("--rgb", type=str, default="CAM_A")
|
||||
ap.add_argument("--cam-b", type=str, default="CAM_B")
|
||||
ap.add_argument("--cam-c", type=str, default="CAM_C")
|
||||
ap.add_argument("--mono-resolution", type=str, default="800p", choices=["400p", "480p", "720p", "800p"])
|
||||
ap.add_argument("--fps", type=float, default=10.0)
|
||||
ap.add_argument("--preview-w", type=int, default=640)
|
||||
ap.add_argument("--preview-h", type=int, default=400)
|
||||
ap.add_argument("--panel-w", type=int, default=500)
|
||||
ap.add_argument("--out-dir", type=str, default="charuco_preview_out")
|
||||
return ap
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main(build_argparser().parse_args())
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,690 @@
|
|||
import argparse
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
from collections import deque
|
||||
from typing import Dict, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import depthai as dai
|
||||
except Exception as e:
|
||||
raise RuntimeError(
|
||||
"Nao consegui importar depthai. Ative o venv correto e instale depthai antes de rodar. "
|
||||
f"Erro original: {e}"
|
||||
)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# OAK-FCC-3P Depth Probe - API v3 style
|
||||
# ------------------------------------------------------------
|
||||
# Este script evita XLinkOut/getOutputQueue, porque seu ambiente DepthAI
|
||||
# nao expoe dai.node.XLinkOut. Ele usa createOutputQueue() direto nas saidas.
|
||||
#
|
||||
# Exemplo:
|
||||
# python -m utils.depth_probe --left CAM_B --right CAM_C --rgb CAM_A --enable-rgb --lrcheck --extended --subpixel --confidence 200 --median 7
|
||||
#
|
||||
# Se depth/disparity parecer invertido ou muito ruim:
|
||||
# python -m utils.depth_probe --left CAM_C --right CAM_B --rgb CAM_A --enable-rgb --lrcheck --extended --subpixel
|
||||
# ============================================================
|
||||
|
||||
|
||||
# ============================================================
|
||||
# DepthAI helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def socket_from_name(name: str):
|
||||
name = str(name).strip().upper()
|
||||
aliases = {
|
||||
"A": "CAM_A",
|
||||
"B": "CAM_B",
|
||||
"C": "CAM_C",
|
||||
"LEFT": "CAM_B",
|
||||
"RIGHT": "CAM_C",
|
||||
"RGB": "CAM_A",
|
||||
}
|
||||
name = aliases.get(name, name)
|
||||
|
||||
if hasattr(dai.CameraBoardSocket, name):
|
||||
return getattr(dai.CameraBoardSocket, name)
|
||||
|
||||
legacy = {
|
||||
"CAM_A": getattr(dai.CameraBoardSocket, "RGB", None),
|
||||
"CAM_B": getattr(dai.CameraBoardSocket, "LEFT", None),
|
||||
"CAM_C": getattr(dai.CameraBoardSocket, "RIGHT", None),
|
||||
}
|
||||
if legacy.get(name) is not None:
|
||||
return legacy[name]
|
||||
|
||||
raise ValueError(f"Socket invalido: {name}. Use CAM_A, CAM_B ou CAM_C.")
|
||||
|
||||
|
||||
|
||||
def create_node(pipeline: dai.Pipeline, node_type):
|
||||
"""Wrapper pequeno para manter o codigo legivel."""
|
||||
return pipeline.create(node_type)
|
||||
|
||||
|
||||
|
||||
def mono_resolution_from_name(name: str):
|
||||
name = str(name).strip().lower()
|
||||
r = dai.MonoCameraProperties.SensorResolution
|
||||
table = {
|
||||
"400p": getattr(r, "THE_400_P", None),
|
||||
"480p": getattr(r, "THE_480_P", None),
|
||||
"720p": getattr(r, "THE_720_P", None),
|
||||
"800p": getattr(r, "THE_800_P", None),
|
||||
}
|
||||
if name not in table or table[name] is None:
|
||||
valid = ", ".join(k for k, v in table.items() if v is not None)
|
||||
raise ValueError(f"Resolucao mono invalida: {name}. Valid={valid}")
|
||||
return table[name]
|
||||
|
||||
|
||||
|
||||
def median_filter_from_name(name: str):
|
||||
name = str(name).strip().upper()
|
||||
|
||||
enum_candidates = []
|
||||
if hasattr(dai, "MedianFilter"):
|
||||
enum_candidates.append(dai.MedianFilter)
|
||||
if hasattr(dai, "StereoDepthProperties") and hasattr(dai.StereoDepthProperties, "MedianFilter"):
|
||||
enum_candidates.append(dai.StereoDepthProperties.MedianFilter)
|
||||
|
||||
key_map = {
|
||||
"OFF": ("MEDIAN_OFF", "KERNEL_NONE", "OFF"),
|
||||
"3": ("KERNEL_3x3", "MEDIAN_3x3"),
|
||||
"5": ("KERNEL_5x5", "MEDIAN_5x5"),
|
||||
"7": ("KERNEL_7x7", "MEDIAN_7x7"),
|
||||
}
|
||||
|
||||
if name not in key_map:
|
||||
raise ValueError("--median deve ser OFF, 3, 5 ou 7")
|
||||
|
||||
for enum in enum_candidates:
|
||||
for attr in key_map[name]:
|
||||
value = getattr(enum, attr, None)
|
||||
if value is not None:
|
||||
return value
|
||||
|
||||
print("[WARN] Esta versao do DepthAI nao expos enum de MedianFilter; seguindo sem aplicar median filter.")
|
||||
return None
|
||||
|
||||
|
||||
|
||||
def set_if_exists(obj, method_name: str, *args) -> bool:
|
||||
fn = getattr(obj, method_name, None)
|
||||
if callable(fn):
|
||||
try:
|
||||
fn(*args)
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[WARN] {method_name} falhou: {e}")
|
||||
return False
|
||||
|
||||
|
||||
|
||||
def apply_stereo_config(stereo, args):
|
||||
# Preset: tenta alguns nomes comuns.
|
||||
try:
|
||||
preset = getattr(dai.node.StereoDepth.PresetMode, "HIGH_DENSITY", None)
|
||||
if preset is None:
|
||||
preset = getattr(dai.node.StereoDepth.PresetMode, "FAST_DENSITY", None)
|
||||
if preset is not None:
|
||||
stereo.setDefaultProfilePreset(preset)
|
||||
except Exception as e:
|
||||
print(f"[WARN] preset StereoDepth nao aplicado: {e}")
|
||||
|
||||
set_if_exists(stereo, "setLeftRightCheck", bool(args.lrcheck))
|
||||
set_if_exists(stereo, "setExtendedDisparity", bool(args.extended))
|
||||
set_if_exists(stereo, "setSubpixel", bool(args.subpixel))
|
||||
|
||||
# Confidence threshold: mudou bastante entre versoes.
|
||||
applied_conf = False
|
||||
applied_conf = set_if_exists(stereo, "setConfidenceThreshold", int(args.confidence)) or applied_conf
|
||||
|
||||
if not applied_conf:
|
||||
try:
|
||||
applied_conf = set_if_exists(stereo.initialConfig, "setConfidenceThreshold", int(args.confidence)) or applied_conf
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Algumas APIs v3 nao tem initialConfig.get(); tentamos manipular config direto se existir.
|
||||
try:
|
||||
cfg = stereo.initialConfig
|
||||
if hasattr(cfg, "costMatching") and hasattr(cfg.costMatching, "confidenceThreshold"):
|
||||
cfg.costMatching.confidenceThreshold = int(args.confidence)
|
||||
applied_conf = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if not applied_conf:
|
||||
print("[WARN] Nao consegui aplicar confidenceThreshold nesta versao. Seguindo com default.")
|
||||
|
||||
median_value = median_filter_from_name(args.median)
|
||||
if median_value is not None:
|
||||
applied_median = False
|
||||
try:
|
||||
applied_median = set_if_exists(stereo.initialConfig, "setMedianFilter", median_value)
|
||||
except Exception:
|
||||
pass
|
||||
if not applied_median:
|
||||
try:
|
||||
cfg = stereo.initialConfig
|
||||
if hasattr(cfg, "postProcessing") and hasattr(cfg.postProcessing, "median"):
|
||||
cfg.postProcessing.median = median_value
|
||||
applied_median = True
|
||||
except Exception:
|
||||
pass
|
||||
if not applied_median:
|
||||
print("[WARN] Nao consegui aplicar median filter nesta versao. Seguindo com default.")
|
||||
|
||||
# Pos-processamento opcional. Tudo defensivo.
|
||||
try:
|
||||
cfg = stereo.initialConfig
|
||||
pp = getattr(cfg, "postProcessing", None)
|
||||
if pp is not None:
|
||||
if hasattr(pp, "speckleFilter"):
|
||||
pp.speckleFilter.enable = bool(args.speckle)
|
||||
pp.speckleFilter.speckleRange = int(args.speckle_range)
|
||||
if hasattr(pp, "temporalFilter"):
|
||||
pp.temporalFilter.enable = bool(args.temporal)
|
||||
if hasattr(pp, "spatialFilter"):
|
||||
pp.spatialFilter.enable = bool(args.spatial)
|
||||
if hasattr(pp.spatialFilter, "holeFillingRadius"):
|
||||
pp.spatialFilter.holeFillingRadius = int(args.hole_filling_radius)
|
||||
if hasattr(pp.spatialFilter, "numIterations"):
|
||||
pp.spatialFilter.numIterations = int(args.spatial_iterations)
|
||||
except Exception as e:
|
||||
print(f"[WARN] Nao consegui aplicar filtros de pos-processamento: {e}")
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Visual helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def normalize_u8(arr: np.ndarray, p_low: float = 1.0, p_high: float = 99.0) -> np.ndarray:
|
||||
x = np.asarray(arr, dtype=np.float32)
|
||||
finite = np.isfinite(x)
|
||||
if not np.any(finite):
|
||||
return np.zeros(x.shape[:2], dtype=np.uint8)
|
||||
vals = x[finite]
|
||||
lo = float(np.percentile(vals, p_low))
|
||||
hi = float(np.percentile(vals, p_high))
|
||||
if hi <= lo + 1e-6:
|
||||
hi = lo + 1.0
|
||||
y = np.clip((x - lo) / (hi - lo), 0.0, 1.0)
|
||||
return (y * 255).astype(np.uint8)
|
||||
|
||||
|
||||
|
||||
def heatmap(arr: np.ndarray, p_low: float = 1.0, p_high: float = 99.0, cmap=cv2.COLORMAP_TURBO) -> np.ndarray:
|
||||
return cv2.applyColorMap(normalize_u8(arr, p_low, p_high), cmap)
|
||||
|
||||
|
||||
|
||||
def put_label(img: np.ndarray, title: str, subtitle: str = "") -> np.ndarray:
|
||||
if img is None:
|
||||
img = np.zeros((300, 400, 3), dtype=np.uint8)
|
||||
if img.ndim == 2:
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
out = img.copy()
|
||||
hbox = 58 if subtitle else 36
|
||||
cv2.rectangle(out, (0, 0), (out.shape[1], hbox), (0, 0, 0), -1)
|
||||
cv2.putText(out, str(title)[:90], (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 255), 2, cv2.LINE_AA)
|
||||
if subtitle:
|
||||
cv2.putText(out, str(subtitle)[:120], (10, 48), cv2.FONT_HERSHEY_SIMPLEX, 0.44, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
return out
|
||||
|
||||
|
||||
|
||||
def resize_keep(img: np.ndarray, width: int) -> np.ndarray:
|
||||
scale = width / img.shape[1]
|
||||
height = max(1, int(img.shape[0] * scale))
|
||||
return cv2.resize(img, (width, height), interpolation=cv2.INTER_AREA)
|
||||
|
||||
|
||||
|
||||
def make_grid(panels, panel_w: int = 430, cols: int = 3) -> np.ndarray:
|
||||
rendered = []
|
||||
for title, img, subtitle in panels:
|
||||
if img is None:
|
||||
img = np.zeros((300, 400, 3), dtype=np.uint8)
|
||||
if img.ndim == 2:
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
small = resize_keep(img, panel_w)
|
||||
rendered.append(put_label(small, title, subtitle))
|
||||
|
||||
if not rendered:
|
||||
return np.zeros((300, 600, 3), dtype=np.uint8)
|
||||
|
||||
max_h = max(x.shape[0] for x in rendered)
|
||||
padded = []
|
||||
for im in rendered:
|
||||
if im.shape[0] < max_h:
|
||||
im = np.vstack([im, np.zeros((max_h - im.shape[0], im.shape[1], 3), dtype=np.uint8)])
|
||||
padded.append(im)
|
||||
|
||||
gap = 10
|
||||
gap_w = np.full((max_h, gap, 3), 22, dtype=np.uint8)
|
||||
filler = np.zeros_like(padded[0])
|
||||
rows = []
|
||||
for i in range(0, len(padded), cols):
|
||||
items = padded[i:i + cols]
|
||||
while len(items) < cols:
|
||||
items.append(filler.copy())
|
||||
row = items[0]
|
||||
for j in range(1, cols):
|
||||
row = np.hstack([row, gap_w, items[j]])
|
||||
rows.append(row)
|
||||
|
||||
gap_h = np.full((gap, rows[0].shape[1], 3), 22, dtype=np.uint8)
|
||||
canvas = rows[0]
|
||||
for row in rows[1:]:
|
||||
canvas = np.vstack([canvas, gap_h, row])
|
||||
return canvas
|
||||
|
||||
|
||||
|
||||
def safe_stats_depth_mm(depth: np.ndarray, min_mm: int, max_mm: int) -> Dict[str, float]:
|
||||
d = np.asarray(depth, dtype=np.float32)
|
||||
valid = np.isfinite(d) & (d > min_mm) & (d < max_mm)
|
||||
total = int(d.size)
|
||||
count = int(np.count_nonzero(valid))
|
||||
if count <= 0:
|
||||
return {
|
||||
"valid_pct": 0.0,
|
||||
"count": 0,
|
||||
"mean_mm": 0.0,
|
||||
"median_mm": 0.0,
|
||||
"p10_mm": 0.0,
|
||||
"p90_mm": 0.0,
|
||||
"std_mm": 0.0,
|
||||
}
|
||||
vals = d[valid]
|
||||
return {
|
||||
"valid_pct": float(count * 100.0 / max(1, total)),
|
||||
"count": count,
|
||||
"mean_mm": float(np.mean(vals)),
|
||||
"median_mm": float(np.median(vals)),
|
||||
"p10_mm": float(np.percentile(vals, 10)),
|
||||
"p90_mm": float(np.percentile(vals, 90)),
|
||||
"std_mm": float(np.std(vals)),
|
||||
}
|
||||
|
||||
|
||||
|
||||
def stats_disparity(disp: np.ndarray) -> Dict[str, float]:
|
||||
d = np.asarray(disp, dtype=np.float32)
|
||||
valid = np.isfinite(d) & (d > 0)
|
||||
total = int(d.size)
|
||||
count = int(np.count_nonzero(valid))
|
||||
if count <= 0:
|
||||
return {"valid_pct": 0.0, "mean": 0.0, "median": 0.0, "p90": 0.0, "std": 0.0}
|
||||
vals = d[valid]
|
||||
return {
|
||||
"valid_pct": float(count * 100.0 / max(1, total)),
|
||||
"mean": float(np.mean(vals)),
|
||||
"median": float(np.median(vals)),
|
||||
"p90": float(np.percentile(vals, 90)),
|
||||
"std": float(np.std(vals)),
|
||||
}
|
||||
|
||||
|
||||
|
||||
def draw_metrics_panel(metrics: Dict[str, float], disp_stats: Dict[str, float], fps: float, args: argparse.Namespace,
|
||||
size: Tuple[int, int] = (900, 260)) -> np.ndarray:
|
||||
w, h = size
|
||||
img = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
lines = [
|
||||
"OAK-FCC-3P depth probe - API v3 queues",
|
||||
f"left={args.left} right={args.right} rgb={args.rgb} | fps={fps:.1f}",
|
||||
f"lrcheck={args.lrcheck} extended={args.extended} subpixel={args.subpixel} median={args.median} confidence={args.confidence}",
|
||||
f"depth valid={metrics['valid_pct']:.1f}% | median={metrics['median_mm']:.0f}mm mean={metrics['mean_mm']:.0f}mm p10={metrics['p10_mm']:.0f} p90={metrics['p90_mm']:.0f} std={metrics['std_mm']:.0f}",
|
||||
f"disp valid={disp_stats['valid_pct']:.1f}% | median={disp_stats['median']:.2f} mean={disp_stats['mean']:.2f} p90={disp_stats['p90']:.2f} std={disp_stats['std']:.2f}",
|
||||
"teclas: Q/ESC sair | S salvar snapshot | H ajuda",
|
||||
"Leitura: heatmap coerente + valid% alto = vale investigar depth. Ruido/sopa = descartar depth metrico.",
|
||||
]
|
||||
y = 28
|
||||
for i, line in enumerate(lines):
|
||||
color = (0, 255, 255) if i == 0 else (235, 235, 235)
|
||||
cv2.putText(img, line[:145], (12, y), cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 1, cv2.LINE_AA)
|
||||
y += 26
|
||||
return img
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Queue helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def create_output_queue(output, name: str, max_size: int = 4, blocking: bool = False):
|
||||
if output is None:
|
||||
return None
|
||||
|
||||
fn = getattr(output, "createOutputQueue", None)
|
||||
if callable(fn):
|
||||
return fn(maxSize=max_size, blocking=blocking)
|
||||
|
||||
raise RuntimeError(
|
||||
f"A saida '{name}' nao possui createOutputQueue(). "
|
||||
"Seu DepthAI parece nao ter XLinkOut, mas tambem nao expos queues v3 nessa saida."
|
||||
)
|
||||
|
||||
|
||||
|
||||
def get_frame(q) -> Optional[np.ndarray]:
|
||||
if q is None:
|
||||
return None
|
||||
try:
|
||||
msg = q.tryGet()
|
||||
except Exception:
|
||||
return None
|
||||
if msg is None:
|
||||
return None
|
||||
|
||||
# ImgFrame normalmente tem getFrame(). Alguns previews coloridos podem ter getCvFrame().
|
||||
try:
|
||||
return msg.getFrame()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
return msg.getCvFrame()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Pipeline
|
||||
# ============================================================
|
||||
|
||||
|
||||
def create_pipeline_and_outputs(args: argparse.Namespace):
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
left = create_node(pipeline, dai.node.MonoCamera)
|
||||
right = create_node(pipeline, dai.node.MonoCamera)
|
||||
|
||||
left.setBoardSocket(socket_from_name(args.left))
|
||||
right.setBoardSocket(socket_from_name(args.right))
|
||||
left.setResolution(mono_resolution_from_name(args.mono_resolution))
|
||||
right.setResolution(mono_resolution_from_name(args.mono_resolution))
|
||||
left.setFps(float(args.fps))
|
||||
right.setFps(float(args.fps))
|
||||
|
||||
stereo = create_node(pipeline, dai.node.StereoDepth)
|
||||
apply_stereo_config(stereo, args)
|
||||
|
||||
left.out.link(stereo.left)
|
||||
right.out.link(stereo.right)
|
||||
|
||||
outputs = {
|
||||
"left": left.out,
|
||||
"right": right.out,
|
||||
"disparity": stereo.disparity,
|
||||
"depth": stereo.depth,
|
||||
"rectified_left": stereo.rectifiedLeft,
|
||||
"rectified_right": stereo.rectifiedRight,
|
||||
}
|
||||
|
||||
nodes = {
|
||||
"left": left,
|
||||
"right": right,
|
||||
"stereo": stereo,
|
||||
}
|
||||
|
||||
if args.enable_rgb:
|
||||
rgb = create_node(pipeline, dai.node.ColorCamera)
|
||||
rgb.setBoardSocket(socket_from_name(args.rgb))
|
||||
rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
|
||||
rgb.setFps(float(args.fps))
|
||||
rgb.setInterleaved(False)
|
||||
rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
|
||||
rgb.setPreviewSize(int(args.rgb_preview_w), int(args.rgb_preview_h))
|
||||
outputs["rgb"] = rgb.preview
|
||||
nodes["rgb"] = rgb
|
||||
|
||||
return pipeline, outputs, nodes
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Runtime
|
||||
# ============================================================
|
||||
|
||||
|
||||
def save_snapshot(out_dir: Path, frames: Dict[str, np.ndarray], metrics: Dict[str, float], disp_stats: Dict[str, float], args: argparse.Namespace):
|
||||
ts = time.strftime("%Y%m%d_%H%M%S")
|
||||
folder = out_dir / f"depth_probe_{ts}"
|
||||
folder.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
for name, frame in frames.items():
|
||||
if frame is None:
|
||||
continue
|
||||
if frame.ndim == 2:
|
||||
if frame.dtype == np.uint16:
|
||||
np.save(str(folder / f"{name}.npy"), frame)
|
||||
cv2.imwrite(str(folder / f"{name}_preview.png"), normalize_u8(frame))
|
||||
else:
|
||||
cv2.imwrite(str(folder / f"{name}.png"), normalize_u8(frame))
|
||||
else:
|
||||
cv2.imwrite(str(folder / f"{name}.png"), frame)
|
||||
|
||||
meta = {
|
||||
"created_at": ts,
|
||||
"args": vars(args),
|
||||
"depth_metrics": metrics,
|
||||
"disparity_metrics": disp_stats,
|
||||
}
|
||||
with open(folder / "metrics.json", "w", encoding="utf-8") as f:
|
||||
json.dump(meta, f, ensure_ascii=False, indent=2)
|
||||
|
||||
print(f"[OK] snapshot salvo em: {folder}")
|
||||
|
||||
|
||||
|
||||
def start_pipeline_v3(pipeline):
|
||||
fn = getattr(pipeline, "start", None)
|
||||
if not callable(fn):
|
||||
raise RuntimeError(
|
||||
"Este ambiente nao tem pipeline.start(). "
|
||||
"Tambem nao tinha XLinkOut. Pode ser uma build DepthAI intermediaria/incompleta."
|
||||
)
|
||||
fn()
|
||||
|
||||
|
||||
|
||||
def stop_pipeline_v3(pipeline):
|
||||
try:
|
||||
fn = getattr(pipeline, "stop", None)
|
||||
if callable(fn):
|
||||
fn()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
|
||||
def pipeline_running(pipeline) -> bool:
|
||||
fn = getattr(pipeline, "isRunning", None)
|
||||
if callable(fn):
|
||||
try:
|
||||
return bool(fn())
|
||||
except Exception:
|
||||
return True
|
||||
return True
|
||||
|
||||
|
||||
|
||||
def main(args: argparse.Namespace):
|
||||
out_dir = Path(args.out_dir)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
pipeline, outputs, _nodes = create_pipeline_and_outputs(args)
|
||||
|
||||
print("[INFO] Pipeline criado em modo API v3/sem XLinkOut.")
|
||||
print(f"[INFO] left={args.left} right={args.right} rgb={args.rgb} enable_rgb={args.enable_rgb}")
|
||||
print("[INFO] Se depth vier ruim, teste invertendo --left/--right.")
|
||||
|
||||
queues = {
|
||||
name: create_output_queue(output, name, max_size=4, blocking=False)
|
||||
for name, output in outputs.items()
|
||||
}
|
||||
|
||||
start_pipeline_v3(pipeline)
|
||||
|
||||
cv2.namedWindow("OAK-FCC-3P Depth Probe", cv2.WINDOW_NORMAL)
|
||||
cv2.resizeWindow("OAK-FCC-3P Depth Probe", 1500, 900)
|
||||
|
||||
last_frames: Dict[str, Optional[np.ndarray]] = {
|
||||
"left": None,
|
||||
"right": None,
|
||||
"rectified_left": None,
|
||||
"rectified_right": None,
|
||||
"disparity": None,
|
||||
"depth": None,
|
||||
"rgb": None,
|
||||
"canvas": None,
|
||||
}
|
||||
|
||||
frame_times = deque(maxlen=40)
|
||||
last_metrics = safe_stats_depth_mm(np.zeros((1, 1), dtype=np.uint16), args.min_depth_mm, args.max_depth_mm)
|
||||
last_disp_stats = stats_disparity(np.zeros((1, 1), dtype=np.float32))
|
||||
|
||||
try:
|
||||
while pipeline_running(pipeline):
|
||||
updated = False
|
||||
|
||||
for name, queue in queues.items():
|
||||
frame = get_frame(queue)
|
||||
if frame is not None:
|
||||
last_frames[name] = frame
|
||||
updated = True
|
||||
|
||||
if not updated:
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key in (27, ord("q"), ord("Q")):
|
||||
break
|
||||
continue
|
||||
|
||||
if last_frames["disparity"] is not None:
|
||||
frame_times.append(time.time())
|
||||
|
||||
if len(frame_times) >= 2:
|
||||
fps = (len(frame_times) - 1) / max(1e-6, frame_times[-1] - frame_times[0])
|
||||
else:
|
||||
fps = 0.0
|
||||
|
||||
left = last_frames["left"]
|
||||
right = last_frames["right"]
|
||||
rect_left = last_frames["rectified_left"]
|
||||
rect_right = last_frames["rectified_right"]
|
||||
disp = last_frames["disparity"]
|
||||
depth = last_frames["depth"]
|
||||
rgb = last_frames["rgb"]
|
||||
|
||||
if disp is None or depth is None or left is None or right is None:
|
||||
continue
|
||||
|
||||
metrics = safe_stats_depth_mm(depth, args.min_depth_mm, args.max_depth_mm)
|
||||
disp_s = stats_disparity(disp)
|
||||
last_metrics = metrics
|
||||
last_disp_stats = disp_s
|
||||
|
||||
depth_f = depth.astype(np.float32)
|
||||
depth_valid = np.where(
|
||||
(depth_f > args.min_depth_mm) & (depth_f < args.max_depth_mm),
|
||||
depth_f,
|
||||
np.nan,
|
||||
)
|
||||
|
||||
disp_hm = heatmap(disp, 1, 99, cv2.COLORMAP_TURBO)
|
||||
depth_hm = heatmap(depth_valid, 1, 99, cv2.COLORMAP_TURBO)
|
||||
valid_mask = np.where(np.isfinite(depth_valid), 255, 0).astype(np.uint8)
|
||||
valid_bgr = cv2.cvtColor(valid_mask, cv2.COLOR_GRAY2BGR)
|
||||
|
||||
base_for_overlay = rect_left if rect_left is not None else left
|
||||
base_bgr = cv2.cvtColor(normalize_u8(base_for_overlay), cv2.COLOR_GRAY2BGR)
|
||||
depth_hm_res = cv2.resize(depth_hm, (base_bgr.shape[1], base_bgr.shape[0]), interpolation=cv2.INTER_AREA)
|
||||
overlay = cv2.addWeighted(base_bgr, 0.55, depth_hm_res, 0.45, 0)
|
||||
|
||||
panels = [
|
||||
("Left mono", normalize_u8(left), f"{args.left}"),
|
||||
("Right mono", normalize_u8(right), f"{args.right}"),
|
||||
("Metrics", draw_metrics_panel(metrics, disp_s, fps, args), ""),
|
||||
("Rectified left", normalize_u8(rect_left), "stereo.rectifiedLeft"),
|
||||
("Rectified right", normalize_u8(rect_right), "stereo.rectifiedRight"),
|
||||
("Disparity heatmap", disp_hm, f"valid={disp_s['valid_pct']:.1f}%"),
|
||||
("Depth heatmap", depth_hm, f"valid={metrics['valid_pct']:.1f}% median={metrics['median_mm']:.0f}mm"),
|
||||
("Valid depth mask", valid_bgr, f"range={args.min_depth_mm}-{args.max_depth_mm}mm"),
|
||||
("Depth overlay", overlay, "heatmap sobre rectified left"),
|
||||
]
|
||||
|
||||
if rgb is not None:
|
||||
panels.append(("RGB preview", rgb, f"{args.rgb}"))
|
||||
|
||||
canvas = make_grid(panels, panel_w=args.panel_w, cols=3)
|
||||
last_frames["canvas"] = canvas
|
||||
cv2.imshow("OAK-FCC-3P Depth Probe", canvas)
|
||||
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key in (27, ord("q"), ord("Q")):
|
||||
break
|
||||
if key in (ord("s"), ord("S")):
|
||||
frames_to_save = {k: v for k, v in last_frames.items() if v is not None}
|
||||
save_snapshot(out_dir, frames_to_save, last_metrics, last_disp_stats, args)
|
||||
if key in (ord("h"), ord("H")):
|
||||
print("\n=== HELP ===")
|
||||
print("Q/ESC : sair")
|
||||
print("S : salvar snapshot")
|
||||
print("Teste tambem invertendo --left/--right se disparity/depth parecer quebrado.")
|
||||
print("===========\n")
|
||||
|
||||
finally:
|
||||
stop_pipeline_v3(pipeline)
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
# ============================================================
|
||||
# CLI
|
||||
# ============================================================
|
||||
|
||||
|
||||
def build_argparser() -> argparse.ArgumentParser:
|
||||
ap = argparse.ArgumentParser(description="Teste de depth/disparity na OAK-FFC-3P usando par mono RE/NIR, sem XLinkOut.")
|
||||
|
||||
ap.add_argument("--left", type=str, default="CAM_B", help="Socket mono esquerda. Ex: CAM_B ou CAM_C")
|
||||
ap.add_argument("--right", type=str, default="CAM_C", help="Socket mono direita. Ex: CAM_C ou CAM_B")
|
||||
ap.add_argument("--rgb", type=str, default="CAM_A", help="Socket RGB opcional.")
|
||||
ap.add_argument("--enable-rgb", action="store_true", help="Tambem mostra preview RGB.")
|
||||
|
||||
ap.add_argument("--mono-resolution", type=str, default="800p", choices=["400p", "480p", "720p", "800p"])
|
||||
ap.add_argument("--fps", type=float, default=10.0)
|
||||
ap.add_argument("--rgb-preview-w", type=int, default=640)
|
||||
ap.add_argument("--rgb-preview-h", type=int, default=400)
|
||||
|
||||
ap.add_argument("--lrcheck", action="store_true", help="Ativa left-right check para remover matches ruins/oclusoes.")
|
||||
ap.add_argument("--extended", action="store_true", help="Ativa extended disparity, util para curto alcance.")
|
||||
ap.add_argument("--subpixel", action="store_true", help="Ativa subpixel disparity, util para suavidade/maior precisao.")
|
||||
ap.add_argument("--confidence", type=int, default=200, help="Confidence threshold do StereoDepth. Tente 180-245.")
|
||||
ap.add_argument("--median", type=str, default="7", choices=["OFF", "3", "5", "7"], help="Filtro de mediana.")
|
||||
|
||||
ap.add_argument("--speckle", action="store_true", help="Ativa speckle filter no post-processing.")
|
||||
ap.add_argument("--speckle-range", type=int, default=50)
|
||||
ap.add_argument("--temporal", action="store_true", help="Ativa temporal filter, se suportado pela versao.")
|
||||
ap.add_argument("--spatial", action="store_true", help="Ativa spatial filter, se suportado pela versao.")
|
||||
ap.add_argument("--hole-filling-radius", type=int, default=2)
|
||||
ap.add_argument("--spatial-iterations", type=int, default=1)
|
||||
|
||||
ap.add_argument("--min-depth-mm", type=int, default=150)
|
||||
ap.add_argument("--max-depth-mm", type=int, default=5000)
|
||||
ap.add_argument("--panel-w", type=int, default=430)
|
||||
ap.add_argument("--out-dir", type=str, default="depth_probe_out")
|
||||
|
||||
return ap
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main(build_argparser().parse_args())
|
||||
|
|
@ -0,0 +1,546 @@
|
|||
import argparse
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import depthai as dai
|
||||
except Exception as e:
|
||||
raise RuntimeError(
|
||||
"Nao consegui importar depthai. Ative o venv correto e instale depthai antes de rodar. "
|
||||
f"Erro original: {e}"
|
||||
)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# OAK-FCC-3P Calibration Probe
|
||||
# ------------------------------------------------------------
|
||||
# Objetivo:
|
||||
# Ler o que existe de calibracao no device OAK/DepthAI:
|
||||
# - cameras conectadas
|
||||
# - sockets / sensores
|
||||
# - intrinsecos por camera, quando disponivel
|
||||
# - distorcao por camera, quando disponivel
|
||||
# - extrinsecos entre pares CAM_A/CAM_B/CAM_C
|
||||
# - baseline estimado, quando a API permitir
|
||||
# - dump JSON bruto da calibracao, quando disponivel
|
||||
#
|
||||
# Uso:
|
||||
# python -m utils.calibration_probe --out_dir calibration_probe_out
|
||||
#
|
||||
# Para escolher device por MXID:
|
||||
# python -m utils.calibration_probe --mx_id 194430108133AC2F00
|
||||
# ============================================================
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Helpers gerais
|
||||
# ============================================================
|
||||
|
||||
|
||||
def to_jsonable(x: Any):
|
||||
if x is None:
|
||||
return None
|
||||
if isinstance(x, (str, int, float, bool)):
|
||||
if isinstance(x, float) and (math.isnan(x) or math.isinf(x)):
|
||||
return None
|
||||
return x
|
||||
if isinstance(x, np.ndarray):
|
||||
return x.tolist()
|
||||
if isinstance(x, (list, tuple)):
|
||||
return [to_jsonable(v) for v in x]
|
||||
if isinstance(x, dict):
|
||||
return {str(k): to_jsonable(v) for k, v in x.items()}
|
||||
try:
|
||||
return str(x)
|
||||
except Exception:
|
||||
return repr(x)
|
||||
|
||||
|
||||
|
||||
def safe_call(label: str, fn, *args, default=None, verbose: bool = False):
|
||||
try:
|
||||
return fn(*args)
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] {label} falhou: {type(e).__name__}: {e}")
|
||||
return default
|
||||
|
||||
|
||||
|
||||
def get_device_id_from_info(dev_info) -> Optional[str]:
|
||||
for name in ("getMxId", "getDeviceId"):
|
||||
try:
|
||||
fn = getattr(dev_info, name, None)
|
||||
if callable(fn):
|
||||
value = fn()
|
||||
if value:
|
||||
return str(value)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
for attr in ("mxid", "deviceId", "name"):
|
||||
try:
|
||||
value = getattr(dev_info, attr, None)
|
||||
if value:
|
||||
return str(value)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return None
|
||||
|
||||
|
||||
|
||||
def resolve_device_info(mx_id: Optional[str] = None):
|
||||
devices = dai.Device.getAllAvailableDevices()
|
||||
if not devices:
|
||||
raise RuntimeError("Nenhum dispositivo DepthAI/OAK encontrado.")
|
||||
|
||||
if not mx_id:
|
||||
return devices[0]
|
||||
|
||||
target = str(mx_id).strip()
|
||||
for dev_info in devices:
|
||||
dev_id = get_device_id_from_info(dev_info)
|
||||
if dev_id == target:
|
||||
return dev_info
|
||||
|
||||
available = [get_device_id_from_info(d) or str(d) for d in devices]
|
||||
raise RuntimeError(f"Device mx_id='{target}' nao encontrado. Disponiveis={available}")
|
||||
|
||||
|
||||
|
||||
def socket_from_name(name: str):
|
||||
name = str(name).strip().upper()
|
||||
aliases = {
|
||||
"A": "CAM_A",
|
||||
"B": "CAM_B",
|
||||
"C": "CAM_C",
|
||||
"D": "CAM_D",
|
||||
"RGB": "CAM_A",
|
||||
"LEFT": "CAM_B",
|
||||
"RIGHT": "CAM_C",
|
||||
}
|
||||
name = aliases.get(name, name)
|
||||
|
||||
if hasattr(dai.CameraBoardSocket, name):
|
||||
return getattr(dai.CameraBoardSocket, name)
|
||||
|
||||
legacy = {
|
||||
"CAM_A": getattr(dai.CameraBoardSocket, "RGB", None),
|
||||
"CAM_B": getattr(dai.CameraBoardSocket, "LEFT", None),
|
||||
"CAM_C": getattr(dai.CameraBoardSocket, "RIGHT", None),
|
||||
"CAM_D": getattr(dai.CameraBoardSocket, "CAM_D", None),
|
||||
}
|
||||
if legacy.get(name) is not None:
|
||||
return legacy[name]
|
||||
|
||||
raise ValueError(f"Socket invalido: {name}")
|
||||
|
||||
|
||||
|
||||
def socket_name(socket_obj) -> str:
|
||||
try:
|
||||
return str(socket_obj.name)
|
||||
except Exception:
|
||||
return str(socket_obj)
|
||||
|
||||
|
||||
|
||||
def matrix_shape_ok(m, rows: int, cols: int) -> bool:
|
||||
try:
|
||||
arr = np.asarray(m, dtype=np.float64)
|
||||
return arr.shape == (rows, cols) and np.all(np.isfinite(arr))
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
|
||||
def flatten_matrix(m):
|
||||
try:
|
||||
return np.asarray(m, dtype=np.float64).tolist()
|
||||
except Exception:
|
||||
return to_jsonable(m)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Calibration read helpers
|
||||
# ============================================================
|
||||
|
||||
|
||||
def read_calibration(device, verbose: bool = False):
|
||||
# Contratos comuns: readCalibration(), readCalibration2().
|
||||
for method in ("readCalibration", "readCalibration2"):
|
||||
fn = getattr(device, method, None)
|
||||
if callable(fn):
|
||||
calib = safe_call(method, fn, default=None, verbose=verbose)
|
||||
if calib is not None:
|
||||
print(f"[OK] Calibracao lida via device.{method}()")
|
||||
return calib, method
|
||||
|
||||
raise RuntimeError("Nao encontrei device.readCalibration/readCalibration2 nesta versao do DepthAI.")
|
||||
|
||||
|
||||
|
||||
def dump_calibration_json(calib, out_dir: Path, verbose: bool = False) -> Dict[str, Any]:
|
||||
"""Tenta extrair dump bruto da calibracao por varios contratos de API."""
|
||||
result = {
|
||||
"available": False,
|
||||
"method": None,
|
||||
"path": None,
|
||||
"data": None,
|
||||
"error": None,
|
||||
}
|
||||
|
||||
# 1) eepromToJson() costuma devolver dict/json.
|
||||
for method in ("eepromToJson", "toJson"):
|
||||
fn = getattr(calib, method, None)
|
||||
if callable(fn):
|
||||
try:
|
||||
data = fn()
|
||||
if isinstance(data, str):
|
||||
try:
|
||||
data_obj = json.loads(data)
|
||||
except Exception:
|
||||
data_obj = data
|
||||
else:
|
||||
data_obj = data
|
||||
path = out_dir / f"calibration_{method}.json"
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(to_jsonable(data_obj), f, ensure_ascii=False, indent=2)
|
||||
result.update({"available": True, "method": method, "path": str(path), "data": to_jsonable(data_obj)})
|
||||
print(f"[OK] Dump bruto salvo via calib.{method}(): {path}")
|
||||
return result
|
||||
except Exception as e:
|
||||
result["error"] = f"{method}: {type(e).__name__}: {e}"
|
||||
if verbose:
|
||||
print(f"[WARN] dump {method} falhou: {e}")
|
||||
|
||||
# 2) Alguns handlers escrevem direto em arquivo.
|
||||
for method in ("saveToJsonFile", "saveCalibrationFile", "saveToFile"):
|
||||
fn = getattr(calib, method, None)
|
||||
if callable(fn):
|
||||
path = out_dir / f"calibration_{method}.json"
|
||||
try:
|
||||
fn(str(path))
|
||||
result.update({"available": True, "method": method, "path": str(path), "data": None})
|
||||
print(f"[OK] Dump bruto salvo via calib.{method}(): {path}")
|
||||
return result
|
||||
except Exception as e:
|
||||
result["error"] = f"{method}: {type(e).__name__}: {e}"
|
||||
if verbose:
|
||||
print(f"[WARN] dump {method} falhou: {e}")
|
||||
|
||||
print("[WARN] Nao consegui gerar dump JSON bruto da calibracao por API conhecida.")
|
||||
return result
|
||||
|
||||
|
||||
|
||||
def get_connected_cameras(device) -> List[Dict[str, Any]]:
|
||||
features = safe_call("getConnectedCameraFeatures", device.getConnectedCameraFeatures, default=[], verbose=False)
|
||||
out = []
|
||||
for f in features:
|
||||
item = {}
|
||||
try:
|
||||
item["socket"] = socket_name(f.socket)
|
||||
except Exception:
|
||||
item["socket"] = None
|
||||
for attr in ("sensorName", "width", "height", "orientation", "supportedTypes"):
|
||||
try:
|
||||
v = getattr(f, attr, None)
|
||||
item[attr] = to_jsonable(v)
|
||||
except Exception:
|
||||
pass
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
|
||||
def get_intrinsics(calib, socket, width: int, height: int, verbose: bool = False):
|
||||
# Contratos comuns:
|
||||
# getCameraIntrinsics(socket)
|
||||
# getCameraIntrinsics(socket, width, height)
|
||||
fn = getattr(calib, "getCameraIntrinsics", None)
|
||||
if not callable(fn):
|
||||
return None, "missing:getCameraIntrinsics"
|
||||
|
||||
for args in ((socket, width, height), (socket,)):
|
||||
try:
|
||||
value = fn(*args)
|
||||
if value is not None:
|
||||
return flatten_matrix(value), f"getCameraIntrinsics{len(args)}args"
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] intrinsics {socket_name(socket)} args={len(args)} falhou: {e}")
|
||||
return None, "failed:getCameraIntrinsics"
|
||||
|
||||
|
||||
|
||||
def get_distortion(calib, socket, verbose: bool = False):
|
||||
for method in ("getDistortionCoefficients", "getDistortionCoeff"):
|
||||
fn = getattr(calib, method, None)
|
||||
if callable(fn):
|
||||
try:
|
||||
value = fn(socket)
|
||||
return to_jsonable(value), method
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] distortion {socket_name(socket)} {method} falhou: {e}")
|
||||
return None, "missing:distortion"
|
||||
|
||||
|
||||
|
||||
def get_fov(calib, socket, verbose: bool = False):
|
||||
fn = getattr(calib, "getFov", None)
|
||||
if callable(fn):
|
||||
try:
|
||||
return float(fn(socket)), "getFov"
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] fov {socket_name(socket)} falhou: {e}")
|
||||
return None, "missing:getFov"
|
||||
|
||||
|
||||
|
||||
def get_extrinsics(calib, src_socket, dst_socket, verbose: bool = False):
|
||||
fn = getattr(calib, "getCameraExtrinsics", None)
|
||||
if not callable(fn):
|
||||
return None, "missing:getCameraExtrinsics"
|
||||
|
||||
# Contratos comuns:
|
||||
# getCameraExtrinsics(src, dst)
|
||||
# getCameraExtrinsics(src, dst, useSpecTranslation)
|
||||
for args in ((src_socket, dst_socket), (src_socket, dst_socket, False), (src_socket, dst_socket, True)):
|
||||
try:
|
||||
value = fn(*args)
|
||||
if value is not None:
|
||||
return flatten_matrix(value), f"getCameraExtrinsics{len(args)}args"
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] extrinsics {socket_name(src_socket)}->{socket_name(dst_socket)} args={len(args)} falhou: {e}")
|
||||
return None, "failed:getCameraExtrinsics"
|
||||
|
||||
|
||||
|
||||
def get_baseline(calib, src_socket, dst_socket, verbose: bool = False):
|
||||
# Varia entre versoes; em algumas, getBaselineDistance(cam1, cam2, useSpecTranslation)
|
||||
fn = getattr(calib, "getBaselineDistance", None)
|
||||
if callable(fn):
|
||||
for args in ((src_socket, dst_socket), (src_socket, dst_socket, False), (src_socket, dst_socket, True)):
|
||||
try:
|
||||
value = fn(*args)
|
||||
if value is not None:
|
||||
return float(value), f"getBaselineDistance{len(args)}args"
|
||||
except Exception as e:
|
||||
if verbose:
|
||||
print(f"[WARN] baseline {socket_name(src_socket)}-{socket_name(dst_socket)} args={len(args)} falhou: {e}")
|
||||
|
||||
# Fallback: calcula norma da translacao da matriz 4x4, se existir.
|
||||
ext, method = get_extrinsics(calib, src_socket, dst_socket, verbose=False)
|
||||
if ext is not None:
|
||||
try:
|
||||
arr = np.asarray(ext, dtype=np.float64)
|
||||
if arr.shape == (4, 4):
|
||||
t = arr[:3, 3]
|
||||
return float(np.linalg.norm(t)), f"norm_translation_from_{method}"
|
||||
except Exception:
|
||||
pass
|
||||
return None, "missing:getBaselineDistance"
|
||||
|
||||
|
||||
|
||||
def inspect_socket(calib, socket_name_str: str, width: int, height: int, verbose: bool = False) -> Dict[str, Any]:
|
||||
socket = socket_from_name(socket_name_str)
|
||||
intr, intr_method = get_intrinsics(calib, socket, width, height, verbose=verbose)
|
||||
dist, dist_method = get_distortion(calib, socket, verbose=verbose)
|
||||
fov, fov_method = get_fov(calib, socket, verbose=verbose)
|
||||
|
||||
intr_ok = matrix_shape_ok(intr, 3, 3)
|
||||
dist_ok = dist is not None
|
||||
|
||||
return {
|
||||
"socket": socket_name_str,
|
||||
"intrinsics": intr,
|
||||
"intrinsics_method": intr_method,
|
||||
"intrinsics_ok": bool(intr_ok),
|
||||
"distortion": dist,
|
||||
"distortion_method": dist_method,
|
||||
"distortion_ok": bool(dist_ok),
|
||||
"fov_deg": fov,
|
||||
"fov_method": fov_method,
|
||||
}
|
||||
|
||||
|
||||
|
||||
def inspect_pair(calib, src_name: str, dst_name: str, verbose: bool = False) -> Dict[str, Any]:
|
||||
src = socket_from_name(src_name)
|
||||
dst = socket_from_name(dst_name)
|
||||
ext, ext_method = get_extrinsics(calib, src, dst, verbose=verbose)
|
||||
base, base_method = get_baseline(calib, src, dst, verbose=verbose)
|
||||
|
||||
ext_ok = matrix_shape_ok(ext, 4, 4)
|
||||
translation = None
|
||||
if ext_ok:
|
||||
try:
|
||||
arr = np.asarray(ext, dtype=np.float64)
|
||||
translation = arr[:3, 3].tolist()
|
||||
except Exception:
|
||||
translation = None
|
||||
|
||||
return {
|
||||
"pair": f"{src_name}->{dst_name}",
|
||||
"src": src_name,
|
||||
"dst": dst_name,
|
||||
"extrinsics": ext,
|
||||
"extrinsics_method": ext_method,
|
||||
"extrinsics_ok": bool(ext_ok),
|
||||
"translation": translation,
|
||||
"baseline": base,
|
||||
"baseline_method": base_method,
|
||||
"baseline_ok": bool(base is not None),
|
||||
}
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Report
|
||||
# ============================================================
|
||||
|
||||
|
||||
def print_summary(report: Dict[str, Any]):
|
||||
print("\n================ CALIBRATION PROBE SUMMARY ================")
|
||||
print(f"device_id : {report.get('device_id')}")
|
||||
print(f"read_method : {report.get('calibration_read_method')}")
|
||||
print(f"dump_json : {report.get('calibration_dump', {}).get('path')}")
|
||||
|
||||
print("\n[Cameras conectadas]")
|
||||
for cam in report.get("connected_cameras", []):
|
||||
print(f" - socket={cam.get('socket')} sensor={cam.get('sensorName')} size={cam.get('width')}x{cam.get('height')}")
|
||||
|
||||
print("\n[Intrinsecos por socket]")
|
||||
for s in report.get("sockets", []):
|
||||
print(
|
||||
f" - {s['socket']}: intrinsics_ok={s['intrinsics_ok']} "
|
||||
f"distortion_ok={s['distortion_ok']} fov={s.get('fov_deg')}"
|
||||
)
|
||||
|
||||
print("\n[Extrinsecos entre pares]")
|
||||
for p in report.get("pairs", []):
|
||||
flag = "OK" if p.get("extrinsics_ok") else "MISSING"
|
||||
print(
|
||||
f" - {p['pair']}: {flag} | baseline={p.get('baseline')} "
|
||||
f"| method={p.get('extrinsics_method')}"
|
||||
)
|
||||
|
||||
# Diagnostico direto para o caso de depth RE/NIR.
|
||||
bc = next((p for p in report.get("pairs", []) if p.get("pair") == "CAM_B->CAM_C"), None)
|
||||
cb = next((p for p in report.get("pairs", []) if p.get("pair") == "CAM_C->CAM_B"), None)
|
||||
|
||||
print("\n[Diagnostico CAM_B/CAM_C para StereoDepth]")
|
||||
if (bc and bc.get("extrinsics_ok")) or (cb and cb.get("extrinsics_ok")):
|
||||
print(" ✅ Existe extrinseco entre CAM_B e CAM_C. O StereoDepth deve ter chance de iniciar.")
|
||||
else:
|
||||
print(" ❌ Nao existe extrinseco CAM_B<->CAM_C legivel pela API.")
|
||||
print(" Isso explica erro: 'There is no available extrinsic calibration between camera ID: 1 and 2'.")
|
||||
print(" Proximo passo: calibrar o par CAM_B/CAM_C ou carregar um calibration.json valido.")
|
||||
|
||||
print("===========================================================\n")
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Main
|
||||
# ============================================================
|
||||
|
||||
|
||||
def main(args: argparse.Namespace):
|
||||
out_dir = Path(args.out_dir)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
dev_info = resolve_device_info(args.mx_id)
|
||||
device_id = get_device_id_from_info(dev_info)
|
||||
|
||||
print(f"[INFO] Abrindo device: {device_id}")
|
||||
|
||||
with dai.Device(dev_info) as device:
|
||||
connected = get_connected_cameras(device)
|
||||
calib, read_method = read_calibration(device, verbose=args.verbose)
|
||||
dump = dump_calibration_json(calib, out_dir=out_dir, verbose=args.verbose)
|
||||
|
||||
sockets = [s.strip().upper() for s in args.sockets.split(",") if s.strip()]
|
||||
pairs = []
|
||||
socket_reports = []
|
||||
|
||||
for s in sockets:
|
||||
try:
|
||||
socket_reports.append(inspect_socket(calib, s, args.width, args.height, verbose=args.verbose))
|
||||
except Exception as e:
|
||||
socket_reports.append({
|
||||
"socket": s,
|
||||
"error": f"{type(e).__name__}: {e}",
|
||||
"intrinsics_ok": False,
|
||||
"distortion_ok": False,
|
||||
})
|
||||
|
||||
for src in sockets:
|
||||
for dst in sockets:
|
||||
if src == dst:
|
||||
continue
|
||||
try:
|
||||
pairs.append(inspect_pair(calib, src, dst, verbose=args.verbose))
|
||||
except Exception as e:
|
||||
pairs.append({
|
||||
"pair": f"{src}->{dst}",
|
||||
"src": src,
|
||||
"dst": dst,
|
||||
"error": f"{type(e).__name__}: {e}",
|
||||
"extrinsics_ok": False,
|
||||
"baseline_ok": False,
|
||||
})
|
||||
|
||||
report = {
|
||||
"created_at": time.strftime("%Y-%m-%d %H:%M:%S"),
|
||||
"device_id": device_id,
|
||||
"calibration_read_method": read_method,
|
||||
"connected_cameras": connected,
|
||||
"sockets_requested": sockets,
|
||||
"width": int(args.width),
|
||||
"height": int(args.height),
|
||||
"calibration_dump": {
|
||||
"available": dump.get("available"),
|
||||
"method": dump.get("method"),
|
||||
"path": dump.get("path"),
|
||||
"error": dump.get("error"),
|
||||
},
|
||||
"sockets": socket_reports,
|
||||
"pairs": pairs,
|
||||
}
|
||||
|
||||
out_path = out_dir / "calibration_probe_report.json"
|
||||
with open(out_path, "w", encoding="utf-8") as f:
|
||||
json.dump(to_jsonable(report), f, ensure_ascii=False, indent=2)
|
||||
|
||||
print(f"[OK] Relatorio salvo em: {out_path}")
|
||||
print_summary(report)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# CLI
|
||||
# ============================================================
|
||||
|
||||
|
||||
def build_argparser() -> argparse.ArgumentParser:
|
||||
ap = argparse.ArgumentParser(description="Inspeciona calibracao EEPROM/JSON do device OAK/DepthAI.")
|
||||
ap.add_argument("--mx_id", type=str, default=None, help="MXID opcional do device.")
|
||||
ap.add_argument("--out_dir", type=str, default="calibration_probe_out", help="Pasta de saida.")
|
||||
ap.add_argument("--sockets", type=str, default="CAM_A,CAM_B,CAM_C", help="Sockets para testar. Ex: CAM_A,CAM_B,CAM_C")
|
||||
ap.add_argument("--width", type=int, default=1280, help="Largura usada ao pedir intrinsecos escalados.")
|
||||
ap.add_argument("--height", type=int, default=800, help="Altura usada ao pedir intrinsecos escalados.")
|
||||
ap.add_argument("--verbose", action="store_true", help="Mostra warnings detalhados de APIs que falharam.")
|
||||
return ap
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main(build_argparser().parse_args())
|
||||
|
|
@ -10,6 +10,10 @@ import numpy as np
|
|||
from core.oak_fcc3_client import OakFcc3Client as MultiSpectralClient
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Utilidades gerais
|
||||
# ============================================================
|
||||
|
||||
def now_str() -> str:
|
||||
return datetime.now().strftime("%Y-%m-%d %H:%M:%S")
|
||||
|
||||
|
|
@ -100,7 +104,7 @@ def build_overlay_fuse(
|
|||
if spec01 is None:
|
||||
return base_bgr
|
||||
|
||||
if calibration_mode == "homography":
|
||||
if calibration_mode in ("homography", "charuco_auto"):
|
||||
warped = apply_homography(spec01, H)
|
||||
else:
|
||||
warped = apply_affine(spec01, dx, dy, theta_deg)
|
||||
|
|
@ -166,9 +170,13 @@ def validate_module_ready(status, frame_type, raw_policy):
|
|||
raise RuntimeError(f"frame_type desconhecido para validação: {frame_type}")
|
||||
|
||||
|
||||
# ============================================================
|
||||
# JSON de calibração
|
||||
# ============================================================
|
||||
|
||||
def default_offsets_payload(args, effective_capture_mode):
|
||||
return {
|
||||
"schema": "manual_multispec_offsets_v2",
|
||||
"schema": "manual_multispec_offsets_v3",
|
||||
"saved_at": now_str(),
|
||||
"frame_type": "RAW_BRUTO",
|
||||
"capture_mode_requested": args.capture_mode,
|
||||
|
|
@ -188,6 +196,8 @@ def default_offsets_payload(args, effective_capture_mode):
|
|||
"re_to_rgb": None,
|
||||
"nir_to_rgb": None,
|
||||
},
|
||||
"homography_metrics": {},
|
||||
"charuco": {},
|
||||
"notes": args.notes or "",
|
||||
}
|
||||
|
||||
|
|
@ -199,12 +209,14 @@ def load_offsets_json(path, args, effective_capture_mode):
|
|||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
|
||||
data.setdefault("schema", "manual_multispec_offsets_v2")
|
||||
data.setdefault("schema", "manual_multispec_offsets_v3")
|
||||
data.setdefault("reference_camera", "rgb")
|
||||
data.setdefault("baseline_mm", args.baseline_mm)
|
||||
data.setdefault("alignment_mode", "manual_affine")
|
||||
data.setdefault("manual_offsets", {})
|
||||
data.setdefault("homographies", {})
|
||||
data.setdefault("homography_metrics", {})
|
||||
data.setdefault("charuco", {})
|
||||
|
||||
data["manual_offsets"].setdefault("re", {"dx": 0, "dy": 0, "theta_deg": 0.0})
|
||||
data["manual_offsets"].setdefault("nir", {"dx": 0, "dy": 0, "theta_deg": 0.0})
|
||||
|
|
@ -224,9 +236,216 @@ def save_offsets_json(path, data):
|
|||
json.dump(data, f, ensure_ascii=False, indent=2)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# ChArUco automático para homografia planar
|
||||
# ============================================================
|
||||
|
||||
def require_aruco():
|
||||
if not hasattr(cv2, "aruco"):
|
||||
raise RuntimeError("cv2.aruco não disponível. Instale opencv-contrib-python no ambiente.")
|
||||
|
||||
|
||||
def get_aruco_dictionary(dict_name: str):
|
||||
require_aruco()
|
||||
if not hasattr(cv2.aruco, dict_name):
|
||||
available = sorted([x for x in dir(cv2.aruco) if x.startswith("DICT_")])
|
||||
raise ValueError(f"Dicionário ArUco inválido: {dict_name}. Disponíveis: {available}")
|
||||
|
||||
dict_id = getattr(cv2.aruco, dict_name)
|
||||
if hasattr(cv2.aruco, "getPredefinedDictionary"):
|
||||
return cv2.aruco.getPredefinedDictionary(dict_id)
|
||||
return cv2.aruco.Dictionary_get(dict_id)
|
||||
|
||||
|
||||
def create_charuco_board(squares_x, squares_y, square_length, marker_length, dictionary):
|
||||
require_aruco()
|
||||
# OpenCV novo: cv2.aruco.CharucoBoard((x, y), squareLength, markerLength, dictionary)
|
||||
if hasattr(cv2.aruco, "CharucoBoard"):
|
||||
try:
|
||||
return cv2.aruco.CharucoBoard((squares_x, squares_y), square_length, marker_length, dictionary)
|
||||
except TypeError:
|
||||
pass
|
||||
|
||||
# OpenCV legado: cv2.aruco.CharucoBoard_create(x, y, squareLength, markerLength, dictionary)
|
||||
if hasattr(cv2.aruco, "CharucoBoard_create"):
|
||||
return cv2.aruco.CharucoBoard_create(squares_x, squares_y, square_length, marker_length, dictionary)
|
||||
|
||||
raise RuntimeError("API ChArUco não encontrada no cv2.aruco deste ambiente.")
|
||||
|
||||
|
||||
def create_detector_params():
|
||||
require_aruco()
|
||||
if hasattr(cv2.aruco, "DetectorParameters"):
|
||||
return cv2.aruco.DetectorParameters()
|
||||
return cv2.aruco.DetectorParameters_create()
|
||||
|
||||
|
||||
def to_gray_u8_for_charuco(img01, equalize=True, invert=False):
|
||||
if img01 is None:
|
||||
return None
|
||||
|
||||
img = np.asarray(img01)
|
||||
|
||||
if img.ndim == 3:
|
||||
if img.shape[2] == 3:
|
||||
gray = cv2.cvtColor(np.clip(img * 255.0, 0, 255).astype(np.uint8), cv2.COLOR_RGB2GRAY)
|
||||
else:
|
||||
gray = img[..., 0]
|
||||
else:
|
||||
gray = img
|
||||
|
||||
if gray.dtype != np.uint8:
|
||||
gray = np.asarray(gray, dtype=np.float32)
|
||||
finite = np.isfinite(gray)
|
||||
if not finite.any():
|
||||
return np.zeros(gray.shape[:2], dtype=np.uint8)
|
||||
|
||||
lo = float(np.percentile(gray[finite], 1.0))
|
||||
hi = float(np.percentile(gray[finite], 99.5))
|
||||
if hi <= lo + 1e-9:
|
||||
hi = lo + 1.0
|
||||
gray = np.clip((gray - lo) / (hi - lo) * 255.0, 0, 255).astype(np.uint8)
|
||||
|
||||
if invert:
|
||||
gray = 255 - gray
|
||||
|
||||
if equalize:
|
||||
try:
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
gray = clahe.apply(gray)
|
||||
except Exception:
|
||||
gray = cv2.equalizeHist(gray)
|
||||
|
||||
return gray
|
||||
|
||||
|
||||
def detect_charuco_points(
|
||||
img01,
|
||||
board,
|
||||
dictionary,
|
||||
detector_params,
|
||||
equalize=True,
|
||||
invert=False,
|
||||
min_markers=4,
|
||||
):
|
||||
"""
|
||||
Retorna dict: charuco_id -> (x, y), além de resumo de detecção.
|
||||
Compatível com APIs nova/legada do OpenCV.
|
||||
"""
|
||||
gray = to_gray_u8_for_charuco(img01, equalize=equalize, invert=invert)
|
||||
if gray is None:
|
||||
return {}, {"markers": 0, "corners": 0, "ok": False}
|
||||
|
||||
# API nova pode ter ArucoDetector, mas detectMarkers continua existindo em quase todos.
|
||||
corners, ids, rejected = cv2.aruco.detectMarkers(gray, dictionary, parameters=detector_params)
|
||||
|
||||
n_markers = 0 if ids is None else int(len(ids))
|
||||
if ids is None or n_markers < min_markers:
|
||||
return {}, {"markers": n_markers, "corners": 0, "ok": False}
|
||||
|
||||
try:
|
||||
cv2.aruco.refineDetectedMarkers(gray, board, corners, ids, rejected)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
ret, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco(
|
||||
markerCorners=corners,
|
||||
markerIds=ids,
|
||||
image=gray,
|
||||
board=board,
|
||||
)
|
||||
|
||||
if charuco_corners is None or charuco_ids is None:
|
||||
return {}, {"markers": n_markers, "corners": 0, "ok": False}
|
||||
|
||||
point_by_id = {}
|
||||
ids_flat = charuco_ids.reshape(-1)
|
||||
pts = charuco_corners.reshape(-1, 2)
|
||||
|
||||
for cid, pt in zip(ids_flat, pts):
|
||||
point_by_id[int(cid)] = (float(pt[0]), float(pt[1]))
|
||||
|
||||
return point_by_id, {
|
||||
"markers": n_markers,
|
||||
"corners": int(len(point_by_id)),
|
||||
"ok": len(point_by_id) >= 4,
|
||||
}
|
||||
|
||||
|
||||
def append_charuco_pairs(accum, role, rgb_points, spec_points, sample_id):
|
||||
common_ids = sorted(set(rgb_points.keys()) & set(spec_points.keys()))
|
||||
added = 0
|
||||
|
||||
for cid in common_ids:
|
||||
spec_pt = spec_points[cid]
|
||||
rgb_pt = rgb_points[cid]
|
||||
accum[role]["spec"].append(spec_pt)
|
||||
accum[role]["rgb"].append(rgb_pt)
|
||||
accum[role]["ids"].append(int(cid))
|
||||
accum[role]["sample_ids"].append(int(sample_id))
|
||||
added += 1
|
||||
|
||||
return added, common_ids
|
||||
|
||||
|
||||
def compute_homography_from_points(src_pts, dst_pts, ransac_reproj_threshold=3.0):
|
||||
if len(src_pts) < 4 or len(dst_pts) < 4 or len(src_pts) != len(dst_pts):
|
||||
return None, None, None
|
||||
|
||||
src = np.array(src_pts, dtype=np.float32)
|
||||
dst = np.array(dst_pts, dtype=np.float32)
|
||||
|
||||
H, status = cv2.findHomography(src, dst, method=cv2.RANSAC, ransacReprojThreshold=float(ransac_reproj_threshold))
|
||||
if H is None:
|
||||
return None, status, None
|
||||
|
||||
projected = cv2.perspectiveTransform(src.reshape(-1, 1, 2), H).reshape(-1, 2)
|
||||
err = np.linalg.norm(projected - dst, axis=1)
|
||||
|
||||
if status is not None:
|
||||
inlier_mask = status.reshape(-1).astype(bool)
|
||||
else:
|
||||
inlier_mask = np.ones(len(err), dtype=bool)
|
||||
|
||||
if inlier_mask.any():
|
||||
err_in = err[inlier_mask]
|
||||
else:
|
||||
err_in = err
|
||||
|
||||
metrics = {
|
||||
"points": int(len(src_pts)),
|
||||
"inliers": int(inlier_mask.sum()),
|
||||
"outliers": int(len(src_pts) - inlier_mask.sum()),
|
||||
"inlier_ratio": float(inlier_mask.sum() / max(len(src_pts), 1)),
|
||||
"mean_error_px": float(np.mean(err_in)) if len(err_in) else None,
|
||||
"median_error_px": float(np.median(err_in)) if len(err_in) else None,
|
||||
"max_error_px": float(np.max(err_in)) if len(err_in) else None,
|
||||
"ransac_reproj_threshold_px": float(ransac_reproj_threshold),
|
||||
}
|
||||
|
||||
return H, status, metrics
|
||||
|
||||
|
||||
def draw_charuco_points_on_panel(board_img, rect, point_by_id, color, max_labels=80):
|
||||
if rect is None or not point_by_id:
|
||||
return
|
||||
|
||||
x0, y0, _, _ = rect
|
||||
for idx, (cid, pt) in enumerate(point_by_id.items()):
|
||||
px = int(x0 + pt[0])
|
||||
py = int(y0 + pt[1])
|
||||
cv2.circle(board_img, (px, py), 4, color, -1)
|
||||
if idx < max_labels:
|
||||
cv2.putText(board_img, str(cid), (px + 5, py - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.38, color, 1, cv2.LINE_AA)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Main
|
||||
# ============================================================
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Calibrador manual de offsets para fusão RGB/RE/NIR a partir do stream RAW_BRUTO.",
|
||||
description="Calibrador de offsets/homografia para fusão RGB/RE/NIR a partir do stream RAW_BRUTO.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||||
)
|
||||
|
||||
|
|
@ -244,6 +463,24 @@ def main():
|
|||
parser.add_argument("--out_json", default="calibration/manual_offsets.json")
|
||||
parser.add_argument("--load_json", default="")
|
||||
parser.add_argument("--notes", default="")
|
||||
parser.add_argument("--module_calibration_json", default="calibration/module_params.json")
|
||||
|
||||
# ChArUco automático
|
||||
parser.add_argument("--charuco_auto", action="store_true", help="Inicia direto no modo de homografia automática por ChArUco.")
|
||||
parser.add_argument("--charuco_dictionary", default="DICT_5X5_100")
|
||||
parser.add_argument("--charuco_squares_x", type=int, default=7)
|
||||
parser.add_argument("--charuco_squares_y", type=int, default=5)
|
||||
parser.add_argument("--charuco_square_length", type=float, default=1.0)
|
||||
parser.add_argument("--charuco_marker_length", type=float, default=0.70)
|
||||
parser.add_argument("--charuco_min_markers", type=int, default=4)
|
||||
parser.add_argument("--charuco_min_common_corners", type=int, default=8)
|
||||
parser.add_argument("--charuco_ransac_px", type=float, default=3.0)
|
||||
parser.add_argument("--charuco_equalize", action="store_true", default=True)
|
||||
parser.add_argument("--charuco_no_equalize", action="store_false", dest="charuco_equalize")
|
||||
parser.add_argument("--charuco_invert_rgb", action="store_true")
|
||||
parser.add_argument("--charuco_invert_re", action="store_true")
|
||||
parser.add_argument("--charuco_invert_nir", action="store_true")
|
||||
parser.add_argument("--charuco_autocalc", action="store_true", help="Calcula H automaticamente após cada captura ChArUco válida.")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
|
|
@ -253,11 +490,38 @@ def main():
|
|||
offsets = offsets_data["manual_offsets"]
|
||||
|
||||
selected_role = "re"
|
||||
calibration_mode = offsets_data.get("alignment_mode", "manual_affine")
|
||||
calibration_mode = "charuco_auto" if args.charuco_auto else offsets_data.get("alignment_mode", "manual_affine")
|
||||
|
||||
selected_points_spec = {"re": [], "nir": []}
|
||||
selected_points_rgb = {"re": [], "nir": []}
|
||||
|
||||
charuco_accum = {
|
||||
"re": {"spec": [], "rgb": [], "ids": [], "sample_ids": []},
|
||||
"nir": {"spec": [], "rgb": [], "ids": [], "sample_ids": []},
|
||||
}
|
||||
charuco_last = {
|
||||
"rgb": {},
|
||||
"re": {},
|
||||
"nir": {},
|
||||
"summary": {},
|
||||
}
|
||||
charuco_sample_id = 0
|
||||
|
||||
dictionary = None
|
||||
board_charuco = None
|
||||
detector_params = None
|
||||
|
||||
if args.charuco_auto:
|
||||
dictionary = get_aruco_dictionary(args.charuco_dictionary)
|
||||
board_charuco = create_charuco_board(
|
||||
args.charuco_squares_x,
|
||||
args.charuco_squares_y,
|
||||
args.charuco_square_length,
|
||||
args.charuco_marker_length,
|
||||
dictionary,
|
||||
)
|
||||
detector_params = create_detector_params()
|
||||
|
||||
panel_rects = {
|
||||
"fuse": None,
|
||||
"rgb": None,
|
||||
|
|
@ -278,7 +542,7 @@ def main():
|
|||
stream_frames_accum = 0
|
||||
last_stream_frame_id = None
|
||||
|
||||
window_name = "Manual Fusion Calibrator"
|
||||
window_name = "Fusion Calibrator - Manual/Homography/ChArUco"
|
||||
|
||||
def inside(rect, px, py):
|
||||
if rect is None:
|
||||
|
|
@ -290,6 +554,21 @@ def main():
|
|||
x0, y0, _, _ = rect
|
||||
return float(px - x0), float(py - y0)
|
||||
|
||||
def ensure_charuco_runtime():
|
||||
nonlocal dictionary, board_charuco, detector_params
|
||||
if dictionary is None:
|
||||
dictionary = get_aruco_dictionary(args.charuco_dictionary)
|
||||
if board_charuco is None:
|
||||
board_charuco = create_charuco_board(
|
||||
args.charuco_squares_x,
|
||||
args.charuco_squares_y,
|
||||
args.charuco_square_length,
|
||||
args.charuco_marker_length,
|
||||
dictionary,
|
||||
)
|
||||
if detector_params is None:
|
||||
detector_params = create_detector_params()
|
||||
|
||||
def on_mouse(event, x, y, flags, param):
|
||||
nonlocal last_msg, last_msg_t, selected_role, calibration_mode
|
||||
|
||||
|
|
@ -329,6 +608,108 @@ def main():
|
|||
|
||||
return None
|
||||
|
||||
def calculate_and_store_h_for_role(role):
|
||||
pts = charuco_accum[role]
|
||||
if len(pts["spec"]) < 4 or len(pts["rgb"]) < 4:
|
||||
return False, f"{role.upper()}: pontos acumulados insuficientes"
|
||||
|
||||
H, status, metrics = compute_homography_from_points(
|
||||
pts["spec"],
|
||||
pts["rgb"],
|
||||
ransac_reproj_threshold=args.charuco_ransac_px,
|
||||
)
|
||||
|
||||
if H is None:
|
||||
return False, f"{role.upper()}: falha ao calcular H"
|
||||
|
||||
offsets_data.setdefault("homographies", {})
|
||||
offsets_data.setdefault("homography_metrics", {})
|
||||
offsets_data["homographies"][f"{role}_to_rgb"] = H.tolist()
|
||||
offsets_data["homography_metrics"][f"{role}_to_rgb"] = metrics
|
||||
|
||||
return True, (
|
||||
f"{role.upper()}: H OK | pts={metrics['points']} | "
|
||||
f"inliers={metrics['inliers']} | err_med={metrics['mean_error_px']:.2f}px"
|
||||
)
|
||||
|
||||
def calculate_charuco_homographies(all_roles=False):
|
||||
roles = ("re", "nir") if all_roles else (selected_role,)
|
||||
messages = []
|
||||
ok_any = False
|
||||
|
||||
for role in roles:
|
||||
ok, msg = calculate_and_store_h_for_role(role)
|
||||
ok_any = ok_any or ok
|
||||
messages.append(msg)
|
||||
|
||||
return ok_any, " | ".join(messages)
|
||||
|
||||
def grab_charuco_sample(rgb01, re01, nir01):
|
||||
nonlocal charuco_sample_id
|
||||
ensure_charuco_runtime()
|
||||
|
||||
rgb_pts, rgb_sum = detect_charuco_points(
|
||||
rgb01,
|
||||
board_charuco,
|
||||
dictionary,
|
||||
detector_params,
|
||||
equalize=args.charuco_equalize,
|
||||
invert=args.charuco_invert_rgb,
|
||||
min_markers=args.charuco_min_markers,
|
||||
)
|
||||
|
||||
re_pts, re_sum = detect_charuco_points(
|
||||
re01,
|
||||
board_charuco,
|
||||
dictionary,
|
||||
detector_params,
|
||||
equalize=args.charuco_equalize,
|
||||
invert=args.charuco_invert_re,
|
||||
min_markers=args.charuco_min_markers,
|
||||
) if re01 is not None else ({}, {"markers": 0, "corners": 0, "ok": False})
|
||||
|
||||
nir_pts, nir_sum = detect_charuco_points(
|
||||
nir01,
|
||||
board_charuco,
|
||||
dictionary,
|
||||
detector_params,
|
||||
equalize=args.charuco_equalize,
|
||||
invert=args.charuco_invert_nir,
|
||||
min_markers=args.charuco_min_markers,
|
||||
) if nir01 is not None else ({}, {"markers": 0, "corners": 0, "ok": False})
|
||||
|
||||
charuco_last["rgb"] = rgb_pts
|
||||
charuco_last["re"] = re_pts
|
||||
charuco_last["nir"] = nir_pts
|
||||
charuco_last["summary"] = {"rgb": rgb_sum, "re": re_sum, "nir": nir_sum}
|
||||
|
||||
if not rgb_pts:
|
||||
return False, "ChArUco: RGB não detectou cantos válidos"
|
||||
|
||||
charuco_sample_id += 1
|
||||
messages = []
|
||||
added_total = 0
|
||||
|
||||
for role, spec_pts in (("re", re_pts), ("nir", nir_pts)):
|
||||
if not spec_pts:
|
||||
messages.append(f"{role.upper()}: sem detecção")
|
||||
continue
|
||||
|
||||
common = sorted(set(rgb_pts.keys()) & set(spec_pts.keys()))
|
||||
if len(common) < args.charuco_min_common_corners:
|
||||
messages.append(f"{role.upper()}: comum={len(common)} < {args.charuco_min_common_corners}")
|
||||
continue
|
||||
|
||||
added, _ = append_charuco_pairs(charuco_accum, role, rgb_pts, spec_pts, charuco_sample_id)
|
||||
added_total += added
|
||||
messages.append(f"{role.upper()}: +{added} pares")
|
||||
|
||||
if args.charuco_autocalc and added_total > 0:
|
||||
_, calc_msg = calculate_charuco_homographies(all_roles=True)
|
||||
messages.append(calc_msg)
|
||||
|
||||
return added_total > 0, "ChArUco sample #{:03d}: {}".format(charuco_sample_id, " | ".join(messages))
|
||||
|
||||
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
|
||||
cv2.setMouseCallback(window_name, on_mouse)
|
||||
|
||||
|
|
@ -342,7 +723,7 @@ def main():
|
|||
output_dtype="uint8",
|
||||
capture_mode=effective_capture_mode,
|
||||
raw_policy=args.raw_policy,
|
||||
module_calibration_json="calibration/module_params.json"
|
||||
module_calibration_json=args.module_calibration_json,
|
||||
) as cam:
|
||||
|
||||
validate_module_ready(cam.get_status(), "RAW_BRUTO", args.raw_policy)
|
||||
|
|
@ -437,12 +818,19 @@ def main():
|
|||
|
||||
spec_pts = len(selected_points_spec[selected_role])
|
||||
rgb_pts = len(selected_points_rgb[selected_role])
|
||||
ch_re_pts = len(charuco_accum["re"]["spec"])
|
||||
ch_nir_pts = len(charuco_accum["nir"]["spec"])
|
||||
ch_sum = charuco_last.get("summary", {}) or {}
|
||||
ch_rgb = ch_sum.get("rgb", {}).get("corners", 0)
|
||||
ch_re = ch_sum.get("re", {}).get("corners", 0)
|
||||
ch_nir = ch_sum.get("nir", {}).get("corners", 0)
|
||||
|
||||
lines_fuse = [
|
||||
f"FUSE: RGB + {active_spec_name}",
|
||||
f"mode={calibration_mode} | selecionada={selected_role.upper()}",
|
||||
f"dx={dx} | dy={dy} | theta={theta_deg:.2f}g | step={args.step} | ang_step={args.angle_step:.2f}g",
|
||||
f"pts_spec={spec_pts} | pts_rgb={rgb_pts} | min=4 | fps_stream={fps_stream:.1f} | fps_view={fps_view:.1f}",
|
||||
f"manual pts_spec={spec_pts} pts_rgb={rgb_pts} | charuco RE={ch_re_pts} NIR={ch_nir_pts}",
|
||||
f"last corners RGB={ch_rgb} RE={ch_re} NIR={ch_nir} | fps_stream={fps_stream:.1f} fps_view={fps_view:.1f}",
|
||||
]
|
||||
overlay_hud(fuse_panel, lines_fuse)
|
||||
|
||||
|
|
@ -490,18 +878,18 @@ def main():
|
|||
board = np.vstack([top, bottom])
|
||||
|
||||
help_lines = [
|
||||
"M=manual_affine | H=homography | clique pares | >=4 pares | SPACE=salva | C=limpa pts | Z=zera sel | X=zera tudo",
|
||||
"A/W/S/D movem | J/L rotacionam | O/P ang_step | I/U remove ponto | ENTER calcula H | TAB alterna RE/NIR | Q/Esc sai",
|
||||
"M=manual | H=homog clique | K=charuco | G=captura charuco | ENTER=calcula H | SPACE=salva | C=limpa pts | V=limpa charuco",
|
||||
"2/3 seleciona | TAB alterna | A/W/S/D movem | J/L rotacionam | I/U remove clique | Z=zera sel | X=zera tudo | Q/Esc sai",
|
||||
]
|
||||
overlay_hud(board, help_lines, x=16, y=board.shape[0] - 44, font_scale=0.55, line_step=20)
|
||||
|
||||
if last_msg and (time.time() - last_msg_t) < 2.5:
|
||||
if last_msg and (time.time() - last_msg_t) < 3.5:
|
||||
cv2.putText(
|
||||
board,
|
||||
last_msg,
|
||||
(16, board.shape[0] - 72),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.7,
|
||||
0.62,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
cv2.LINE_AA,
|
||||
|
|
@ -529,6 +917,11 @@ def main():
|
|||
cv2.circle(board, (px, py), 5, color_rgb, -1)
|
||||
cv2.putText(board, str(idx + 1), (px + 6, py - 6), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_rgb, 1, cv2.LINE_AA)
|
||||
|
||||
if calibration_mode == "charuco_auto":
|
||||
draw_charuco_points_on_panel(board, panel_rects["rgb"], charuco_last.get("rgb", {}), (0, 255, 0))
|
||||
draw_charuco_points_on_panel(board, panel_rects["re"], charuco_last.get("re", {}), (0, 255, 255))
|
||||
draw_charuco_points_on_panel(board, panel_rects["nir"], charuco_last.get("nir", {}), (255, 255, 0))
|
||||
|
||||
if args.preview_scale != 1.0:
|
||||
board = cv2.resize(
|
||||
board,
|
||||
|
|
@ -557,34 +950,72 @@ def main():
|
|||
elif k in (ord("h"), ord("H")):
|
||||
calibration_mode = "homography"
|
||||
offsets_data["alignment_mode"] = calibration_mode
|
||||
last_msg = "Modo: homography"
|
||||
last_msg = "Modo: homography manual por cliques"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k in (ord("k"), ord("K")):
|
||||
ensure_charuco_runtime()
|
||||
calibration_mode = "charuco_auto"
|
||||
offsets_data["alignment_mode"] = calibration_mode
|
||||
last_msg = "Modo: charuco_auto"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k in (ord("g"), ord("G")):
|
||||
if decoded_last:
|
||||
rgb_id, rgb01 = get_image_by_role(decoded_last, "rgb")
|
||||
_, re01 = get_image_by_role(decoded_last, "re")
|
||||
_, nir01 = get_image_by_role(decoded_last, "nir")
|
||||
if rgb01 is not None:
|
||||
base_h, base_w = rgb01.shape[:2]
|
||||
re01 = resize_if_needed(re01, (base_h, base_w))
|
||||
nir01 = resize_if_needed(nir01, (base_h, base_w))
|
||||
ok, msg = grab_charuco_sample(rgb01, re01, nir01)
|
||||
last_msg = msg
|
||||
else:
|
||||
last_msg = "ChArUco: RGB indisponível"
|
||||
else:
|
||||
last_msg = "ChArUco: sem decoded_last"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k in (ord("c"), ord("C")):
|
||||
selected_points_spec[selected_role] = []
|
||||
selected_points_rgb[selected_role] = []
|
||||
last_msg = f"Pontos limpos: {selected_role.upper()}"
|
||||
last_msg = f"Pontos manuais limpos: {selected_role.upper()}"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k in (ord("v"), ord("V")):
|
||||
charuco_accum[selected_role] = {"spec": [], "rgb": [], "ids": [], "sample_ids": []}
|
||||
last_msg = f"Pontos ChArUco limpos: {selected_role.upper()}"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k == 13:
|
||||
spec_pts = selected_points_spec[selected_role]
|
||||
rgb_pts = selected_points_rgb[selected_role]
|
||||
|
||||
if len(spec_pts) >= 4 and len(rgb_pts) >= 4 and len(spec_pts) == len(rgb_pts):
|
||||
src = np.array(spec_pts, dtype=np.float32)
|
||||
dst = np.array(rgb_pts, dtype=np.float32)
|
||||
|
||||
H, status = cv2.findHomography(src, dst, method=cv2.RANSAC)
|
||||
if H is not None:
|
||||
offsets_data.setdefault("homographies", {})
|
||||
offsets_data["homographies"][f"{selected_role}_to_rgb"] = H.tolist()
|
||||
inliers = int(status.sum()) if status is not None else len(spec_pts)
|
||||
last_msg = f"H calculada para {selected_role.upper()} | pts={len(spec_pts)} | inliers={inliers}"
|
||||
else:
|
||||
last_msg = f"Falha ao calcular H para {selected_role.upper()}"
|
||||
if calibration_mode == "charuco_auto":
|
||||
ok, msg = calculate_charuco_homographies(all_roles=False)
|
||||
last_msg = msg
|
||||
else:
|
||||
last_msg = f"{selected_role.upper()}: precisa de >=4 pares e mesmo numero de pontos"
|
||||
spec_pts = selected_points_spec[selected_role]
|
||||
rgb_pts = selected_points_rgb[selected_role]
|
||||
|
||||
if len(spec_pts) >= 4 and len(rgb_pts) >= 4 and len(spec_pts) == len(rgb_pts):
|
||||
H, status, metrics = compute_homography_from_points(
|
||||
spec_pts,
|
||||
rgb_pts,
|
||||
ransac_reproj_threshold=args.charuco_ransac_px,
|
||||
)
|
||||
if H is not None:
|
||||
offsets_data.setdefault("homographies", {})
|
||||
offsets_data.setdefault("homography_metrics", {})
|
||||
offsets_data["homographies"][f"{selected_role}_to_rgb"] = H.tolist()
|
||||
offsets_data["homography_metrics"][f"{selected_role}_to_rgb"] = metrics
|
||||
last_msg = (
|
||||
f"H manual calculada para {selected_role.upper()} | "
|
||||
f"pts={metrics['points']} | inliers={metrics['inliers']} | "
|
||||
f"err_med={metrics['mean_error_px']:.2f}px"
|
||||
)
|
||||
else:
|
||||
last_msg = f"Falha ao calcular H para {selected_role.upper()}"
|
||||
else:
|
||||
last_msg = f"{selected_role.upper()}: precisa de >=4 pares e mesmo numero de pontos"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k == ord("2"):
|
||||
|
|
@ -633,12 +1064,29 @@ def main():
|
|||
|
||||
offsets_data["manual_offsets"] = offsets
|
||||
offsets_data.setdefault("homographies", {})
|
||||
offsets_data["schema"] = "manual_multispec_offsets_v2"
|
||||
offsets_data.setdefault("homography_metrics", {})
|
||||
offsets_data["schema"] = "manual_multispec_offsets_v3"
|
||||
offsets_data["reference_camera"] = "rgb"
|
||||
offsets_data["alignment_mode"] = calibration_mode
|
||||
offsets_data["homography_calibration_size"] = [int(base_w), int(base_h)]
|
||||
offsets_data["charuco"] = {
|
||||
"enabled": calibration_mode == "charuco_auto",
|
||||
"dictionary": args.charuco_dictionary,
|
||||
"squares_x": int(args.charuco_squares_x),
|
||||
"squares_y": int(args.charuco_squares_y),
|
||||
"square_length": float(args.charuco_square_length),
|
||||
"marker_length": float(args.charuco_marker_length),
|
||||
"samples": int(charuco_sample_id),
|
||||
"accumulated_pairs": {
|
||||
"re": int(len(charuco_accum["re"]["spec"])),
|
||||
"nir": int(len(charuco_accum["nir"]["spec"])),
|
||||
},
|
||||
"same_physical_plane_required": True,
|
||||
"notes": "Homografias ChArUco estimadas para um único plano físico. Mova o tabuleiro no plano do solo; não misture inclinações/alturas para uma H única.",
|
||||
}
|
||||
|
||||
save_offsets_json(args.out_json, offsets_data)
|
||||
last_msg = f"Offsets salvos em: {args.out_json}"
|
||||
last_msg = f"Calibração salva em: {args.out_json}"
|
||||
last_msg_t = time.time()
|
||||
|
||||
elif k in (ord("+"), ord("=")):
|
||||
|
|
@ -709,8 +1157,8 @@ def main():
|
|||
|
||||
finally:
|
||||
cv2.destroyAllWindows()
|
||||
print("Fim da calibração manual.")
|
||||
print("Fim da calibração de fusão.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
main()
|
||||
|
|
|
|||
Loading…
Reference in New Issue