1035 lines
35 KiB
Python
1035 lines
35 KiB
Python
|
import argparse
|
||
|
|
import json
|
||
|
|
import re
|
||
|
|
from pathlib import Path
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# RAW10 / image conversion
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def unpack_raw10_packed(raw: bytes, width: int, height: int) -> np.ndarray:
|
||
|
|
"""
|
||
|
|
RAW10 packed padrão:
|
||
|
|
5 bytes = 4 pixels de 10 bits.
|
||
|
|
Retorna uint16 HxW com valores 0..1023.
|
||
|
|
"""
|
||
|
|
arr = np.frombuffer(raw, dtype=np.uint8)
|
||
|
|
|
||
|
|
pixel_count = width * height
|
||
|
|
expected_bytes = (pixel_count // 4) * 5
|
||
|
|
|
||
|
|
if pixel_count % 4 != 0:
|
||
|
|
raise RuntimeError(f"width*height precisa ser múltiplo de 4. Recebido: {pixel_count}")
|
||
|
|
|
||
|
|
if arr.size < expected_bytes:
|
||
|
|
raise RuntimeError(
|
||
|
|
f"RAW10 menor que esperado. bytes={arr.size}, esperado={expected_bytes}, "
|
||
|
|
f"width={width}, height={height}"
|
||
|
|
)
|
||
|
|
|
||
|
|
arr = arr[:expected_bytes]
|
||
|
|
groups = arr.reshape(-1, 5).astype(np.uint16)
|
||
|
|
|
||
|
|
p0 = (groups[:, 0] << 2) | ((groups[:, 4] >> 0) & 0x03)
|
||
|
|
p1 = (groups[:, 1] << 2) | ((groups[:, 4] >> 2) & 0x03)
|
||
|
|
p2 = (groups[:, 2] << 2) | ((groups[:, 4] >> 4) & 0x03)
|
||
|
|
p3 = (groups[:, 3] << 2) | ((groups[:, 4] >> 6) & 0x03)
|
||
|
|
|
||
|
|
out = np.empty(groups.shape[0] * 4, dtype=np.uint16)
|
||
|
|
out[0::4] = p0
|
||
|
|
out[1::4] = p1
|
||
|
|
out[2::4] = p2
|
||
|
|
out[3::4] = p3
|
||
|
|
|
||
|
|
return out.reshape(height, width)
|
||
|
|
|
||
|
|
|
||
|
|
def normalize_to_u8(img: np.ndarray, p_low=1.0, p_high=99.0) -> np.ndarray:
|
||
|
|
arr = img.astype(np.float32)
|
||
|
|
valid = np.isfinite(arr)
|
||
|
|
|
||
|
|
if np.count_nonzero(valid) < 20:
|
||
|
|
return np.zeros(arr.shape[:2], dtype=np.uint8)
|
||
|
|
|
||
|
|
vals = arr[valid]
|
||
|
|
lo = np.percentile(vals, p_low)
|
||
|
|
hi = np.percentile(vals, p_high)
|
||
|
|
|
||
|
|
out = (arr - lo) / (hi - lo + 1e-6)
|
||
|
|
out = np.clip(out, 0.0, 1.0)
|
||
|
|
return (out * 255).astype(np.uint8)
|
||
|
|
|
||
|
|
|
||
|
|
def debayer_raw10_to_bgr_u8(raw10: np.ndarray, bayer: str) -> np.ndarray:
|
||
|
|
gray_u8 = normalize_to_u8(raw10)
|
||
|
|
bayer = bayer.upper()
|
||
|
|
|
||
|
|
code_map = {
|
||
|
|
"RGGB": cv2.COLOR_BayerRG2BGR,
|
||
|
|
"BGGR": cv2.COLOR_BayerBG2BGR,
|
||
|
|
"GRBG": cv2.COLOR_BayerGR2BGR,
|
||
|
|
"GBRG": cv2.COLOR_BayerGB2BGR,
|
||
|
|
}
|
||
|
|
|
||
|
|
if bayer not in code_map:
|
||
|
|
raise RuntimeError(f"Bayer pattern não suportado: {bayer}")
|
||
|
|
|
||
|
|
return cv2.cvtColor(gray_u8, code_map[bayer])
|
||
|
|
|
||
|
|
|
||
|
|
def raw10_bin_to_gray(path: Path, width: int, height: int, *, is_rgb: bool, bayer: str, use_clahe=True) -> np.ndarray:
|
||
|
|
raw = path.read_bytes()
|
||
|
|
raw10 = unpack_raw10_packed(raw, width, height)
|
||
|
|
|
||
|
|
if is_rgb:
|
||
|
|
bgr = debayer_raw10_to_bgr_u8(raw10, bayer=bayer)
|
||
|
|
gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
|
||
|
|
else:
|
||
|
|
gray = normalize_to_u8(raw10)
|
||
|
|
|
||
|
|
if use_clahe:
|
||
|
|
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||
|
|
gray = clahe.apply(gray)
|
||
|
|
|
||
|
|
return gray
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Metadata
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def find_meta(root_dir: Path) -> Path | None:
|
||
|
|
candidates = list(root_dir.rglob("meta.json")) + list(root_dir.rglob("metadata.json"))
|
||
|
|
return candidates[0] if candidates else None
|
||
|
|
|
||
|
|
|
||
|
|
def extract_camera_info_from_meta(meta: dict, cam_key: str):
|
||
|
|
for root_key in ["camera_info", "cameras", "camera_meta", "payload_sources_info"]:
|
||
|
|
root = meta.get(root_key)
|
||
|
|
if isinstance(root, dict):
|
||
|
|
info = root.get(cam_key)
|
||
|
|
if isinstance(info, dict):
|
||
|
|
return info
|
||
|
|
|
||
|
|
stack = [meta]
|
||
|
|
while stack:
|
||
|
|
obj = stack.pop()
|
||
|
|
if isinstance(obj, dict):
|
||
|
|
if cam_key in obj and isinstance(obj[cam_key], dict):
|
||
|
|
return obj[cam_key]
|
||
|
|
for v in obj.values():
|
||
|
|
if isinstance(v, (dict, list)):
|
||
|
|
stack.append(v)
|
||
|
|
elif isinstance(obj, list):
|
||
|
|
for v in obj:
|
||
|
|
if isinstance(v, (dict, list)):
|
||
|
|
stack.append(v)
|
||
|
|
|
||
|
|
return None
|
||
|
|
|
||
|
|
|
||
|
|
def try_get_width_height_from_meta(root_dir: Path, cam_key: str):
|
||
|
|
meta_path = find_meta(root_dir)
|
||
|
|
if meta_path is None:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
try:
|
||
|
|
meta = json.loads(meta_path.read_text(encoding="utf-8"))
|
||
|
|
except Exception:
|
||
|
|
meta = json.loads(meta_path.read_text(encoding="latin-1"))
|
||
|
|
|
||
|
|
info = extract_camera_info_from_meta(meta, cam_key)
|
||
|
|
if not info:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
width = info.get("width") or info.get("w") or info.get("sensor_width") or info.get("frame_width")
|
||
|
|
height = info.get("height") or info.get("h") or info.get("sensor_height") or info.get("frame_height")
|
||
|
|
|
||
|
|
if width is None or height is None:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
return int(width), int(height)
|
||
|
|
|
||
|
|
|
||
|
|
def resolve_width_height(args, cam_key: str):
|
||
|
|
if args.width > 0 and args.height > 0:
|
||
|
|
return args.width, args.height
|
||
|
|
|
||
|
|
w, h = try_get_width_height_from_meta(Path(args.root_dir), cam_key)
|
||
|
|
if w and h:
|
||
|
|
return w, h
|
||
|
|
|
||
|
|
raise RuntimeError(
|
||
|
|
f"Não consegui descobrir width/height para {cam_key}. "
|
||
|
|
f"Passe manualmente: --width 1280 --height 800"
|
||
|
|
)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Pairing / triplets
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def clean_stem_for_pair(path: Path, cam_key: str):
|
||
|
|
s = path.stem
|
||
|
|
|
||
|
|
variants = [cam_key, cam_key.lower(), cam_key.replace("_", ""), cam_key.replace("_", "").lower()]
|
||
|
|
for v in variants:
|
||
|
|
s = s.replace(v, "")
|
||
|
|
|
||
|
|
s = re.sub(r"[_\-\s]+", "_", s).strip("_").lower()
|
||
|
|
return s
|
||
|
|
|
||
|
|
|
||
|
|
def find_cam_bins(root_dir: Path, cam_key: str):
|
||
|
|
return sorted([p for p in root_dir.rglob("*.bin") if cam_key.lower() in p.name.lower()])
|
||
|
|
|
||
|
|
|
||
|
|
def find_triplets(root_dir: Path, cams: list[str]):
|
||
|
|
by_cam = {cam: find_cam_bins(root_dir, cam) for cam in cams}
|
||
|
|
key_maps = {}
|
||
|
|
|
||
|
|
for cam, paths in by_cam.items():
|
||
|
|
m = {}
|
||
|
|
for p in paths:
|
||
|
|
key = clean_stem_for_pair(p, cam)
|
||
|
|
m[(p.parent, key)] = p
|
||
|
|
m.setdefault((None, key), p)
|
||
|
|
key_maps[cam] = m
|
||
|
|
|
||
|
|
ref_cam = cams[0]
|
||
|
|
triplets = []
|
||
|
|
|
||
|
|
for ref_path in by_cam[ref_cam]:
|
||
|
|
key = clean_stem_for_pair(ref_path, ref_cam)
|
||
|
|
folder = ref_path.parent
|
||
|
|
|
||
|
|
item = {ref_cam: ref_path}
|
||
|
|
ok = True
|
||
|
|
|
||
|
|
for cam in cams[1:]:
|
||
|
|
p = key_maps[cam].get((folder, key)) or key_maps[cam].get((None, key))
|
||
|
|
if p is None:
|
||
|
|
same_folder = [x for x in by_cam[cam] if x.parent == folder]
|
||
|
|
if len(same_folder) == 1:
|
||
|
|
p = same_folder[0]
|
||
|
|
|
||
|
|
if p is None:
|
||
|
|
ok = False
|
||
|
|
break
|
||
|
|
|
||
|
|
item[cam] = p
|
||
|
|
|
||
|
|
if ok:
|
||
|
|
triplets.append(item)
|
||
|
|
|
||
|
|
return triplets, by_cam
|
||
|
|
|
||
|
|
|
||
|
|
def resolve_homography_triplet(triplets: list[dict], homo_ref_frame: str | None, rgb_cam: str):
|
||
|
|
"""
|
||
|
|
Resolve qual triplet será usado como plano de referência.
|
||
|
|
|
||
|
|
Aceita:
|
||
|
|
- caminho completo para qualquer .bin do triplet
|
||
|
|
- nome do arquivo CAM_A/CAM_B/CAM_C
|
||
|
|
- stem parcial sem _CAM_X
|
||
|
|
- índice numérico, ex: "0", "12"
|
||
|
|
|
||
|
|
Se homo_ref_frame vier vazio, retorna None.
|
||
|
|
"""
|
||
|
|
if not homo_ref_frame:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
ref = str(homo_ref_frame).strip()
|
||
|
|
|
||
|
|
if ref.isdigit():
|
||
|
|
idx = int(ref)
|
||
|
|
if idx < 0 or idx >= len(triplets):
|
||
|
|
raise RuntimeError(f"--homo_ref_frame índice fora do range: {idx}. Total={len(triplets)}")
|
||
|
|
return triplets[idx], f"index:{idx}"
|
||
|
|
|
||
|
|
ref_path = Path(ref)
|
||
|
|
ref_name = ref_path.name.lower()
|
||
|
|
ref_stem = ref_path.stem.lower()
|
||
|
|
|
||
|
|
for idx, item in enumerate(triplets):
|
||
|
|
for cam, path in item.items():
|
||
|
|
p_name = path.name.lower()
|
||
|
|
p_stem = path.stem.lower()
|
||
|
|
clean = clean_stem_for_pair(path, cam).lower()
|
||
|
|
|
||
|
|
if ref_name == p_name or ref_stem == p_stem or ref_stem == clean or ref.lower() in p_stem:
|
||
|
|
return item, f"match:{path.name}"
|
||
|
|
|
||
|
|
raise RuntimeError(f"Não encontrei triplet correspondente a --homo_ref_frame={homo_ref_frame}")
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# ChArUco helpers
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def get_aruco_dict(dict_name: str):
|
||
|
|
aruco = cv2.aruco
|
||
|
|
mapping = {
|
||
|
|
"4X4_50": aruco.DICT_4X4_50,
|
||
|
|
"4X4_100": aruco.DICT_4X4_100,
|
||
|
|
"4X4_250": aruco.DICT_4X4_250,
|
||
|
|
"4X4_1000": aruco.DICT_4X4_1000,
|
||
|
|
"5X5_50": aruco.DICT_5X5_50,
|
||
|
|
"5X5_100": aruco.DICT_5X5_100,
|
||
|
|
"5X5_250": aruco.DICT_5X5_250,
|
||
|
|
"5X5_1000": aruco.DICT_5X5_1000,
|
||
|
|
"6X6_50": aruco.DICT_6X6_50,
|
||
|
|
"6X6_100": aruco.DICT_6X6_100,
|
||
|
|
"6X6_250": aruco.DICT_6X6_250,
|
||
|
|
"6X6_1000": aruco.DICT_6X6_1000,
|
||
|
|
}
|
||
|
|
key = dict_name.upper()
|
||
|
|
if key not in mapping:
|
||
|
|
raise RuntimeError(f"Dicionário ArUco não suportado: {dict_name}")
|
||
|
|
|
||
|
|
if hasattr(aruco, "getPredefinedDictionary"):
|
||
|
|
return aruco.getPredefinedDictionary(mapping[key])
|
||
|
|
return aruco.Dictionary_get(mapping[key])
|
||
|
|
|
||
|
|
|
||
|
|
def create_charuco_board(squares_x, squares_y, square_length, marker_length, aruco_dict):
|
||
|
|
aruco = cv2.aruco
|
||
|
|
if hasattr(aruco, "CharucoBoard"):
|
||
|
|
try:
|
||
|
|
return aruco.CharucoBoard((squares_x, squares_y), square_length, marker_length, aruco_dict)
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
if hasattr(aruco, "CharucoBoard_create"):
|
||
|
|
return aruco.CharucoBoard_create(squares_x, squares_y, square_length, marker_length, aruco_dict)
|
||
|
|
raise RuntimeError("Sua versão do OpenCV não tem CharucoBoard/CharucoBoard_create.")
|
||
|
|
|
||
|
|
|
||
|
|
def get_board_corners(board):
|
||
|
|
if hasattr(board, "getChessboardCorners"):
|
||
|
|
return np.array(board.getChessboardCorners(), dtype=np.float32)
|
||
|
|
if hasattr(board, "chessboardCorners"):
|
||
|
|
return np.array(board.chessboardCorners, dtype=np.float32)
|
||
|
|
raise RuntimeError("Não consegui acessar chessboardCorners do ChArUco board.")
|
||
|
|
|
||
|
|
|
||
|
|
def create_detector_params():
|
||
|
|
aruco = cv2.aruco
|
||
|
|
if hasattr(aruco, "DetectorParameters"):
|
||
|
|
return aruco.DetectorParameters()
|
||
|
|
if hasattr(aruco, "DetectorParameters_create"):
|
||
|
|
return aruco.DetectorParameters_create()
|
||
|
|
return None
|
||
|
|
|
||
|
|
|
||
|
|
def detect_charuco(gray: np.ndarray, board, aruco_dict, min_corners: int):
|
||
|
|
aruco = cv2.aruco
|
||
|
|
params = create_detector_params()
|
||
|
|
|
||
|
|
if hasattr(aruco, "CharucoDetector"):
|
||
|
|
try:
|
||
|
|
detector = aruco.CharucoDetector(board)
|
||
|
|
charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
|
||
|
|
if charuco_corners is None or charuco_ids is None:
|
||
|
|
return None, None
|
||
|
|
corners = np.array(charuco_corners, dtype=np.float32).reshape(-1, 2)
|
||
|
|
ids = np.array(charuco_ids, dtype=np.int32).reshape(-1)
|
||
|
|
if len(ids) < min_corners:
|
||
|
|
return None, None
|
||
|
|
return corners, ids
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
if params is not None:
|
||
|
|
marker_corners, marker_ids, rejected = aruco.detectMarkers(gray, aruco_dict, parameters=params)
|
||
|
|
else:
|
||
|
|
marker_corners, marker_ids, rejected = aruco.detectMarkers(gray, aruco_dict)
|
||
|
|
|
||
|
|
if marker_ids is None or len(marker_ids) == 0:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
try:
|
||
|
|
aruco.refineDetectedMarkers(gray, board, marker_corners, marker_ids, rejected)
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(marker_corners, marker_ids, gray, board)
|
||
|
|
if charuco_corners is None or charuco_ids is None:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
corners = np.array(charuco_corners, dtype=np.float32).reshape(-1, 2)
|
||
|
|
ids = np.array(charuco_ids, dtype=np.int32).reshape(-1)
|
||
|
|
|
||
|
|
if len(ids) < min_corners:
|
||
|
|
return None, None
|
||
|
|
|
||
|
|
return corners, ids
|
||
|
|
|
||
|
|
|
||
|
|
def common_points_multi(detections: dict, board_corners: np.ndarray, min_common: int):
|
||
|
|
maps = {}
|
||
|
|
for cam, det in detections.items():
|
||
|
|
corners, ids = det
|
||
|
|
maps[cam] = {int(i): corners[k] for k, i in enumerate(ids)}
|
||
|
|
|
||
|
|
common_ids = None
|
||
|
|
for m in maps.values():
|
||
|
|
ids = set(m.keys())
|
||
|
|
common_ids = ids if common_ids is None else (common_ids & ids)
|
||
|
|
|
||
|
|
common_ids = sorted(common_ids or [])
|
||
|
|
if len(common_ids) < min_common:
|
||
|
|
return None, None, common_ids
|
||
|
|
|
||
|
|
max_id = len(board_corners) - 1
|
||
|
|
common_ids = [cid for cid in common_ids if 0 <= cid <= max_id]
|
||
|
|
if len(common_ids) < min_common:
|
||
|
|
return None, None, common_ids
|
||
|
|
|
||
|
|
obj = np.array([board_corners[cid] for cid in common_ids], dtype=np.float32).reshape(-1, 1, 3)
|
||
|
|
imgpoints = {}
|
||
|
|
for cam, m in maps.items():
|
||
|
|
imgpoints[cam] = np.array([m[cid] for cid in common_ids], dtype=np.float32).reshape(-1, 1, 2)
|
||
|
|
|
||
|
|
return obj, imgpoints, common_ids
|
||
|
|
|
||
|
|
|
||
|
|
def common_points_pair(corners_src, ids_src, corners_dst, ids_dst, min_common: int):
|
||
|
|
if corners_src is None or ids_src is None or corners_dst is None or ids_dst is None:
|
||
|
|
return None, None, []
|
||
|
|
|
||
|
|
map_src = {int(i): corners_src[k] for k, i in enumerate(ids_src)}
|
||
|
|
map_dst = {int(i): corners_dst[k] for k, i in enumerate(ids_dst)}
|
||
|
|
|
||
|
|
common_ids = sorted(set(map_src.keys()) & set(map_dst.keys()))
|
||
|
|
if len(common_ids) < min_common:
|
||
|
|
return None, None, common_ids
|
||
|
|
|
||
|
|
pts_src = np.array([map_src[i] for i in common_ids], dtype=np.float32).reshape(-1, 2)
|
||
|
|
pts_dst = np.array([map_dst[i] for i in common_ids], dtype=np.float32).reshape(-1, 2)
|
||
|
|
return pts_src, pts_dst, common_ids
|
||
|
|
|
||
|
|
|
||
|
|
def draw_debug_panel(gray_by_cam, detections, common_count, accepted, title):
|
||
|
|
panels = []
|
||
|
|
for cam, gray in gray_by_cam.items():
|
||
|
|
bgr = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
|
||
|
|
corners, ids = detections.get(cam, (None, None))
|
||
|
|
n = 0 if ids is None else len(ids)
|
||
|
|
|
||
|
|
if corners is not None and ids is not None:
|
||
|
|
corners_draw = np.array(corners, dtype=np.float32).reshape(-1, 1, 2)
|
||
|
|
ids_draw = np.array(ids, dtype=np.int32).reshape(-1, 1)
|
||
|
|
try:
|
||
|
|
cv2.aruco.drawDetectedCornersCharuco(bgr, corners_draw, ids_draw, (0, 255, 0))
|
||
|
|
except Exception:
|
||
|
|
for p in corners:
|
||
|
|
cv2.circle(bgr, tuple(np.round(p).astype(int)), 3, (0, 255, 0), -1)
|
||
|
|
|
||
|
|
cv2.putText(bgr, f"{cam} corners={n}", (20, 35), cv2.FONT_HERSHEY_SIMPLEX, 0.85, (255, 255, 255), 2, cv2.LINE_AA)
|
||
|
|
panels.append(bgr)
|
||
|
|
|
||
|
|
h = min(p.shape[0] for p in panels)
|
||
|
|
panels = [cv2.resize(p, (int(p.shape[1] * h / p.shape[0]), h), interpolation=cv2.INTER_AREA) for p in panels]
|
||
|
|
dbg = np.hstack(panels)
|
||
|
|
|
||
|
|
color = (0, 255, 0) if accepted else (0, 0, 255)
|
||
|
|
cv2.putText(dbg, f"{title} COMMON={common_count} {'ACCEPTED' if accepted else 'REJECTED'}", (20, dbg.shape[0] - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.9, color, 2, cv2.LINE_AA)
|
||
|
|
return dbg
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Calibration helpers
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def calibrate_single_camera(objpoints, imgpoints, image_size):
|
||
|
|
ret, K, D, rvecs, tvecs = cv2.calibrateCamera(
|
||
|
|
objectPoints=objpoints,
|
||
|
|
imagePoints=imgpoints,
|
||
|
|
imageSize=image_size,
|
||
|
|
cameraMatrix=None,
|
||
|
|
distCoeffs=None,
|
||
|
|
flags=0,
|
||
|
|
)
|
||
|
|
return ret, K, D, rvecs, tvecs
|
||
|
|
|
||
|
|
|
||
|
|
def stereo_calibrate_fixed(objpoints, img1, img2, K1, D1, K2, D2, image_size):
|
||
|
|
flags = cv2.CALIB_FIX_INTRINSIC
|
||
|
|
ret, K1o, D1o, K2o, D2o, R, T, E, F = cv2.stereoCalibrate(
|
||
|
|
objectPoints=objpoints,
|
||
|
|
imagePoints1=img1,
|
||
|
|
imagePoints2=img2,
|
||
|
|
cameraMatrix1=K1,
|
||
|
|
distCoeffs1=D1,
|
||
|
|
cameraMatrix2=K2,
|
||
|
|
distCoeffs2=D2,
|
||
|
|
imageSize=image_size,
|
||
|
|
flags=flags,
|
||
|
|
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 200, 1e-7),
|
||
|
|
)
|
||
|
|
return ret, K1o, D1o, K2o, D2o, R, T, E, F
|
||
|
|
|
||
|
|
|
||
|
|
def invert_pose(R, T):
|
||
|
|
R_inv = R.T
|
||
|
|
T_inv = -R_inv @ T
|
||
|
|
return R_inv, T_inv
|
||
|
|
|
||
|
|
|
||
|
|
def stereo_rectify_maps(K1, D1, K2, D2, image_size, R, T, alpha=0):
|
||
|
|
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
|
||
|
|
cameraMatrix1=K1,
|
||
|
|
distCoeffs1=D1,
|
||
|
|
cameraMatrix2=K2,
|
||
|
|
distCoeffs2=D2,
|
||
|
|
imageSize=image_size,
|
||
|
|
R=R,
|
||
|
|
T=T,
|
||
|
|
flags=cv2.CALIB_ZERO_DISPARITY,
|
||
|
|
alpha=alpha,
|
||
|
|
)
|
||
|
|
|
||
|
|
map1x, map1y = cv2.initUndistortRectifyMap(K1, D1, R1, P1, image_size, cv2.CV_32FC1)
|
||
|
|
map2x, map2y = cv2.initUndistortRectifyMap(K2, D2, R2, P2, image_size, cv2.CV_32FC1)
|
||
|
|
|
||
|
|
return {
|
||
|
|
"R1": R1,
|
||
|
|
"R2": R2,
|
||
|
|
"P1": P1,
|
||
|
|
"P2": P2,
|
||
|
|
"Q": Q,
|
||
|
|
"roi1": np.array(roi1),
|
||
|
|
"roi2": np.array(roi2),
|
||
|
|
"map1x": map1x,
|
||
|
|
"map1y": map1y,
|
||
|
|
"map2x": map2x,
|
||
|
|
"map2y": map2y,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Planar homography helpers
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def compute_planar_homography_from_triplet(
|
||
|
|
triplet: dict,
|
||
|
|
cams: list[str],
|
||
|
|
cam_roles: dict,
|
||
|
|
sizes: dict,
|
||
|
|
image_size: tuple[int, int],
|
||
|
|
args,
|
||
|
|
board,
|
||
|
|
aruco_dict,
|
||
|
|
):
|
||
|
|
"""
|
||
|
|
Calcula H_RE_to_RGB e H_NIR_to_RGB usando UM triplet de referência.
|
||
|
|
O plano físico desse frame vira o plano de referência da fusão planar.
|
||
|
|
"""
|
||
|
|
rgb_cam = args.rgb_cam
|
||
|
|
re_cam = args.re_cam
|
||
|
|
nir_cam = args.nir_cam
|
||
|
|
|
||
|
|
gray_by_cam = {}
|
||
|
|
detections = {}
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
w, h = sizes[cam]
|
||
|
|
is_rgb = cam_roles[cam] == "rgb"
|
||
|
|
gray = raw10_bin_to_gray(
|
||
|
|
triplet[cam],
|
||
|
|
width=w,
|
||
|
|
height=h,
|
||
|
|
is_rgb=is_rgb,
|
||
|
|
bayer=args.rgb_bayer,
|
||
|
|
use_clahe=not args.no_clahe,
|
||
|
|
)
|
||
|
|
|
||
|
|
if gray.shape[::-1] != image_size:
|
||
|
|
gray = cv2.resize(gray, image_size, interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
gray_by_cam[cam] = gray
|
||
|
|
detections[cam] = detect_charuco(gray, board, aruco_dict, min_corners=args.homo_min_corners)
|
||
|
|
|
||
|
|
rgb_corners, rgb_ids = detections[rgb_cam]
|
||
|
|
re_corners, re_ids = detections[re_cam]
|
||
|
|
nir_corners, nir_ids = detections[nir_cam]
|
||
|
|
|
||
|
|
def compute_one(src_cam, src_corners, src_ids):
|
||
|
|
pts_src, pts_rgb, common_ids = common_points_pair(
|
||
|
|
src_corners,
|
||
|
|
src_ids,
|
||
|
|
rgb_corners,
|
||
|
|
rgb_ids,
|
||
|
|
min_common=args.homo_min_common,
|
||
|
|
)
|
||
|
|
|
||
|
|
if pts_src is None:
|
||
|
|
raise RuntimeError(
|
||
|
|
f"Pontos comuns insuficientes para {src_cam}->{rgb_cam}: "
|
||
|
|
f"common={len(common_ids)} min={args.homo_min_common}"
|
||
|
|
)
|
||
|
|
|
||
|
|
H, mask = cv2.findHomography(pts_src, pts_rgb, cv2.RANSAC, args.homo_ransac_thresh)
|
||
|
|
if H is None:
|
||
|
|
raise RuntimeError(f"cv2.findHomography falhou para {src_cam}->{rgb_cam}")
|
||
|
|
|
||
|
|
inliers = int(np.count_nonzero(mask)) if mask is not None else 0
|
||
|
|
return H, mask, common_ids, inliers
|
||
|
|
|
||
|
|
H_re, mask_re, common_re, inliers_re = compute_one(re_cam, re_corners, re_ids)
|
||
|
|
H_nir, mask_nir, common_nir, inliers_nir = compute_one(nir_cam, nir_corners, nir_ids)
|
||
|
|
|
||
|
|
image_w, image_h = image_size
|
||
|
|
ones = np.ones((image_h, image_w), dtype=np.uint8) * 255
|
||
|
|
|
||
|
|
overlap_re_to_rgb = cv2.warpPerspective(
|
||
|
|
ones,
|
||
|
|
H_re,
|
||
|
|
(image_w, image_h),
|
||
|
|
flags=cv2.INTER_NEAREST,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
overlap_nir_to_rgb = cv2.warpPerspective(
|
||
|
|
ones,
|
||
|
|
H_nir,
|
||
|
|
(image_w, image_h),
|
||
|
|
flags=cv2.INTER_NEAREST,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
common_overlap_rgb = cv2.bitwise_and(overlap_re_to_rgb, overlap_nir_to_rgb)
|
||
|
|
|
||
|
|
stats = {
|
||
|
|
"rgb_corners": 0 if rgb_ids is None else len(rgb_ids),
|
||
|
|
"re_corners": 0 if re_ids is None else len(re_ids),
|
||
|
|
"nir_corners": 0 if nir_ids is None else len(nir_ids),
|
||
|
|
"re_common": len(common_re),
|
||
|
|
"nir_common": len(common_nir),
|
||
|
|
"re_inliers": inliers_re,
|
||
|
|
"nir_inliers": inliers_nir,
|
||
|
|
"overlap_re_pct": float(np.mean(overlap_re_to_rgb > 0) * 100.0),
|
||
|
|
"overlap_nir_pct": float(np.mean(overlap_nir_to_rgb > 0) * 100.0),
|
||
|
|
"overlap_common_pct": float(np.mean(common_overlap_rgb > 0) * 100.0),
|
||
|
|
}
|
||
|
|
|
||
|
|
return {
|
||
|
|
"H_RE_to_RGB": H_re,
|
||
|
|
"H_NIR_to_RGB": H_nir,
|
||
|
|
"mask_RE_to_RGB": mask_re,
|
||
|
|
"mask_NIR_to_RGB": mask_nir,
|
||
|
|
"common_ids_RE_to_RGB": np.array(common_re, dtype=np.int32),
|
||
|
|
"common_ids_NIR_to_RGB": np.array(common_nir, dtype=np.int32),
|
||
|
|
"overlap_RE_to_RGB": overlap_re_to_rgb,
|
||
|
|
"overlap_NIR_to_RGB": overlap_nir_to_rgb,
|
||
|
|
"overlap_common_RGB": common_overlap_rgb,
|
||
|
|
"stats": stats,
|
||
|
|
"gray_by_cam": gray_by_cam,
|
||
|
|
"detections": detections,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Main
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def main():
|
||
|
|
parser = argparse.ArgumentParser()
|
||
|
|
parser.add_argument("--root_dir", default="calibration/stereo_dataset", required=True)
|
||
|
|
parser.add_argument("--out_dir", default="calibration/multicam_charuco_calib_out")
|
||
|
|
|
||
|
|
parser.add_argument("--rgb_cam", default="CAM_A")
|
||
|
|
parser.add_argument("--re_cam", default="CAM_B")
|
||
|
|
parser.add_argument("--nir_cam", default="CAM_C")
|
||
|
|
parser.add_argument("--ref_cam", default="CAM_A", help="Referência global. Recomendo CAM_A/RGB.")
|
||
|
|
|
||
|
|
parser.add_argument("--width", type=int, default=1280)
|
||
|
|
parser.add_argument("--height", type=int, default=800)
|
||
|
|
parser.add_argument("--rgb_bayer", default="BGGR")
|
||
|
|
|
||
|
|
parser.add_argument("--squares_x", type=int, default=13)
|
||
|
|
parser.add_argument("--squares_y", type=int, default=7)
|
||
|
|
parser.add_argument("--square_length", type=float, default=0.031)
|
||
|
|
parser.add_argument("--marker_length", type=float, default=0.023)
|
||
|
|
parser.add_argument("--aruco_dict", default="4X4_50")
|
||
|
|
parser.add_argument("--rectify_alpha", type=float, default=0.0)
|
||
|
|
|
||
|
|
parser.add_argument("--min_corners", type=int, default=40)
|
||
|
|
parser.add_argument("--min_common", type=int, default=40)
|
||
|
|
|
||
|
|
# Homografia planar de referência.
|
||
|
|
parser.add_argument(
|
||
|
|
"--homo_ref_frame",
|
||
|
|
default=None,
|
||
|
|
help=(
|
||
|
|
"Triplet usado como plano de referência para H_RE_to_RGB e H_NIR_to_RGB. "
|
||
|
|
"Aceita índice, nome parcial/stem ou caminho de um .bin do triplet. "
|
||
|
|
"Se omitido, não salva homografias planares."
|
||
|
|
),
|
||
|
|
)
|
||
|
|
parser.add_argument("--homo_min_corners", type=int, default=30)
|
||
|
|
parser.add_argument("--homo_min_common", type=int, default=20)
|
||
|
|
parser.add_argument("--homo_ransac_thresh", type=float, default=3.0)
|
||
|
|
|
||
|
|
parser.add_argument("--no_clahe", action="store_true")
|
||
|
|
parser.add_argument("--show", action="store_true")
|
||
|
|
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
root_dir = Path(args.root_dir)
|
||
|
|
out_dir = Path(args.out_dir)
|
||
|
|
debug_dir = out_dir / "debug"
|
||
|
|
out_dir.mkdir(parents=True, exist_ok=True)
|
||
|
|
debug_dir.mkdir(parents=True, exist_ok=True)
|
||
|
|
|
||
|
|
cams = [args.rgb_cam, args.nir_cam, args.re_cam]
|
||
|
|
if args.ref_cam not in cams:
|
||
|
|
raise RuntimeError(f"ref_cam precisa estar em {cams}. Recebido: {args.ref_cam}")
|
||
|
|
|
||
|
|
cam_roles = {
|
||
|
|
args.rgb_cam: "rgb",
|
||
|
|
args.nir_cam: "nir",
|
||
|
|
args.re_cam: "re",
|
||
|
|
}
|
||
|
|
|
||
|
|
sizes = {}
|
||
|
|
for cam in cams:
|
||
|
|
w, h = resolve_width_height(args, cam)
|
||
|
|
sizes[cam] = (w, h)
|
||
|
|
|
||
|
|
image_w = min(w for w, h in sizes.values())
|
||
|
|
image_h = min(h for w, h in sizes.values())
|
||
|
|
image_size = (image_w, image_h)
|
||
|
|
|
||
|
|
print(f"[INFO] root_dir={root_dir}")
|
||
|
|
print(f"[INFO] cams={cams} ref_cam={args.ref_cam}")
|
||
|
|
for cam in cams:
|
||
|
|
print(f"[INFO] {cam} role={cam_roles[cam]} size={sizes[cam]}")
|
||
|
|
print(f"[INFO] image_size usado={image_size}")
|
||
|
|
print(f"[INFO] ChArUco squares={args.squares_x}x{args.squares_y}")
|
||
|
|
print(f"[INFO] square_length={args.square_length}")
|
||
|
|
print(f"[INFO] marker_length={args.marker_length}")
|
||
|
|
print(f"[INFO] rectify_alpha={args.rectify_alpha}")
|
||
|
|
|
||
|
|
triplets, by_cam = find_triplets(root_dir, cams)
|
||
|
|
for cam in cams:
|
||
|
|
print(f"[INFO] arquivos {cam}: {len(by_cam[cam])}")
|
||
|
|
print(f"[INFO] triplets encontrados: {len(triplets)}")
|
||
|
|
|
||
|
|
if len(triplets) < 8:
|
||
|
|
raise RuntimeError("Poucos triplets encontrados. Verifique nomes dos arquivos CAM_A/B/C.")
|
||
|
|
|
||
|
|
aruco_dict = get_aruco_dict(args.aruco_dict)
|
||
|
|
board = create_charuco_board(args.squares_x, args.squares_y, args.square_length, args.marker_length, aruco_dict)
|
||
|
|
board_corners = get_board_corners(board)
|
||
|
|
|
||
|
|
# Dados por câmera para calibração individual.
|
||
|
|
single_objpoints = {cam: [] for cam in cams}
|
||
|
|
single_imgpoints = {cam: [] for cam in cams}
|
||
|
|
|
||
|
|
# Dados globais com pontos comuns nas 3 câmeras.
|
||
|
|
multi_objpoints = []
|
||
|
|
multi_imgpoints = {cam: [] for cam in cams}
|
||
|
|
|
||
|
|
accepted = 0
|
||
|
|
rejected = 0
|
||
|
|
|
||
|
|
for idx, item in enumerate(triplets):
|
||
|
|
print(f"[{idx + 1}/{len(triplets)}] " + " | ".join([f"{cam}={item[cam].name}" for cam in cams]))
|
||
|
|
|
||
|
|
try:
|
||
|
|
gray_by_cam = {}
|
||
|
|
detections = {}
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
w, h = sizes[cam]
|
||
|
|
is_rgb = cam_roles[cam] == "rgb"
|
||
|
|
gray = raw10_bin_to_gray(
|
||
|
|
item[cam],
|
||
|
|
width=w,
|
||
|
|
height=h,
|
||
|
|
is_rgb=is_rgb,
|
||
|
|
bayer=args.rgb_bayer,
|
||
|
|
use_clahe=not args.no_clahe,
|
||
|
|
)
|
||
|
|
|
||
|
|
if gray.shape[::-1] != image_size:
|
||
|
|
gray = cv2.resize(gray, image_size, interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
gray_by_cam[cam] = gray
|
||
|
|
corners, ids = detect_charuco(gray, board, aruco_dict, min_corners=args.min_corners)
|
||
|
|
detections[cam] = (corners, ids)
|
||
|
|
|
||
|
|
counts = {cam: (0 if detections[cam][1] is None else len(detections[cam][1])) for cam in cams}
|
||
|
|
|
||
|
|
if any(detections[cam][0] is None for cam in cams):
|
||
|
|
print(" [REJECT] detect insuficiente: " + ", ".join([f"{cam}={counts[cam]}" for cam in cams]))
|
||
|
|
dbg = draw_debug_panel(gray_by_cam, detections, 0, False, f"triplet_{idx:04d}")
|
||
|
|
cv2.imwrite(str(debug_dir / f"triplet_{idx:04d}_rejected.png"), dbg)
|
||
|
|
rejected += 1
|
||
|
|
continue
|
||
|
|
|
||
|
|
obj, imgpoints_by_cam, common_ids = common_points_multi(detections, board_corners, min_common=args.min_common)
|
||
|
|
if obj is None:
|
||
|
|
print(f" [REJECT] comuns insuficientes nas 3 cams: common={len(common_ids)}")
|
||
|
|
dbg = draw_debug_panel(gray_by_cam, detections, len(common_ids), False, f"triplet_{idx:04d}")
|
||
|
|
cv2.imwrite(str(debug_dir / f"triplet_{idx:04d}_rejected.png"), dbg)
|
||
|
|
rejected += 1
|
||
|
|
continue
|
||
|
|
|
||
|
|
multi_objpoints.append(obj)
|
||
|
|
for cam in cams:
|
||
|
|
multi_imgpoints[cam].append(imgpoints_by_cam[cam])
|
||
|
|
single_objpoints[cam].append(obj.copy())
|
||
|
|
single_imgpoints[cam].append(imgpoints_by_cam[cam].copy())
|
||
|
|
|
||
|
|
accepted += 1
|
||
|
|
dbg = draw_debug_panel(gray_by_cam, detections, len(common_ids), True, f"triplet_{idx:04d}")
|
||
|
|
cv2.imwrite(str(debug_dir / f"triplet_{idx:04d}_accepted.png"), dbg)
|
||
|
|
|
||
|
|
if args.show:
|
||
|
|
cv2.imshow("debug", dbg)
|
||
|
|
key = cv2.waitKey(1) & 0xFF
|
||
|
|
if key in [27, ord("q"), ord("Q")]:
|
||
|
|
break
|
||
|
|
|
||
|
|
print(" [OK] " + ", ".join([f"{cam}={counts[cam]}" for cam in cams]) + f", common3={len(common_ids)}")
|
||
|
|
|
||
|
|
except Exception as e:
|
||
|
|
print(f" [ERRO] {e}")
|
||
|
|
rejected += 1
|
||
|
|
|
||
|
|
if args.show:
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print(f"[INFO] triplets aceitos: {accepted}")
|
||
|
|
print(f"[INFO] triplets rejeitados: {rejected}")
|
||
|
|
|
||
|
|
if accepted < 8:
|
||
|
|
raise RuntimeError(f"Poucos triplets aceitos: {accepted}. Ideal: 20-40+ bons.")
|
||
|
|
|
||
|
|
K = {}
|
||
|
|
D = {}
|
||
|
|
rms_single = {}
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
print(f"[CALIB] Calibrando câmera {cam} ({cam_roles[cam]})...")
|
||
|
|
ret, K_cam, D_cam, _, _ = calibrate_single_camera(single_objpoints[cam], single_imgpoints[cam], image_size)
|
||
|
|
rms_single[cam] = ret
|
||
|
|
K[cam] = K_cam
|
||
|
|
D[cam] = D_cam
|
||
|
|
print(f"[RESULT] RMS {cam}: {ret:.6f}")
|
||
|
|
|
||
|
|
# Pares principais: RGB-NIR, RGB-RE, RE-NIR.
|
||
|
|
pairs = [
|
||
|
|
(args.rgb_cam, args.nir_cam),
|
||
|
|
(args.rgb_cam, args.re_cam),
|
||
|
|
(args.re_cam, args.nir_cam),
|
||
|
|
]
|
||
|
|
|
||
|
|
pair_results = {}
|
||
|
|
|
||
|
|
for cam1, cam2 in pairs:
|
||
|
|
print(f"[CALIB] Stereo {cam1} -> {cam2}...")
|
||
|
|
ret, K1o, D1o, K2o, D2o, R, T, E, F = stereo_calibrate_fixed(
|
||
|
|
multi_objpoints,
|
||
|
|
multi_imgpoints[cam1],
|
||
|
|
multi_imgpoints[cam2],
|
||
|
|
K[cam1],
|
||
|
|
D[cam1],
|
||
|
|
K[cam2],
|
||
|
|
D[cam2],
|
||
|
|
image_size,
|
||
|
|
)
|
||
|
|
|
||
|
|
rect = stereo_rectify_maps(K1o, D1o, K2o, D2o, image_size, R, T, alpha=args.rectify_alpha)
|
||
|
|
|
||
|
|
key = f"{cam1}_{cam2}"
|
||
|
|
pair_results[key] = {
|
||
|
|
"cam1": cam1,
|
||
|
|
"cam2": cam2,
|
||
|
|
"rms": ret,
|
||
|
|
"K1": K1o,
|
||
|
|
"D1": D1o,
|
||
|
|
"K2": K2o,
|
||
|
|
"D2": D2o,
|
||
|
|
"R": R,
|
||
|
|
"T": T,
|
||
|
|
"E": E,
|
||
|
|
"F": F,
|
||
|
|
"rect": rect,
|
||
|
|
}
|
||
|
|
|
||
|
|
print(f"[RESULT] RMS stereo {key}: {ret:.6f}")
|
||
|
|
print(f"[RESULT] T {key}: {T.ravel()}")
|
||
|
|
|
||
|
|
# Extrínsecos para ref_cam.
|
||
|
|
# Pela convenção do stereoCalibrate: X_cam2 = R X_cam1 + T.
|
||
|
|
extr_R_to_ref = {}
|
||
|
|
extr_T_to_ref = {}
|
||
|
|
|
||
|
|
extr_R_to_ref[args.ref_cam] = np.eye(3, dtype=np.float64)
|
||
|
|
extr_T_to_ref[args.ref_cam] = np.zeros((3, 1), dtype=np.float64)
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
if cam == args.ref_cam:
|
||
|
|
continue
|
||
|
|
|
||
|
|
direct_key = f"{args.ref_cam}_{cam}"
|
||
|
|
inverse_key = f"{cam}_{args.ref_cam}"
|
||
|
|
|
||
|
|
if direct_key in pair_results:
|
||
|
|
# X_cam = R X_ref + T. Queremos X_ref = R_inv X_cam + T_inv.
|
||
|
|
R_ref_to_cam = pair_results[direct_key]["R"]
|
||
|
|
T_ref_to_cam = pair_results[direct_key]["T"]
|
||
|
|
R_cam_to_ref, T_cam_to_ref = invert_pose(R_ref_to_cam, T_ref_to_cam)
|
||
|
|
elif inverse_key in pair_results:
|
||
|
|
# X_ref = R X_cam + T.
|
||
|
|
R_cam_to_ref = pair_results[inverse_key]["R"]
|
||
|
|
T_cam_to_ref = pair_results[inverse_key]["T"]
|
||
|
|
else:
|
||
|
|
raise RuntimeError(f"Não encontrei par para relacionar {cam} com {args.ref_cam}")
|
||
|
|
|
||
|
|
extr_R_to_ref[cam] = R_cam_to_ref
|
||
|
|
extr_T_to_ref[cam] = T_cam_to_ref
|
||
|
|
|
||
|
|
# Homografia planar opcional.
|
||
|
|
homo_result = None
|
||
|
|
homo_triplet = None
|
||
|
|
homo_ref_resolved = ""
|
||
|
|
|
||
|
|
if args.homo_ref_frame:
|
||
|
|
print("")
|
||
|
|
print(f"[HOMO] Resolvendo frame de referência planar: {args.homo_ref_frame}")
|
||
|
|
homo_triplet, homo_ref_resolved = resolve_homography_triplet(triplets, args.homo_ref_frame, args.rgb_cam)
|
||
|
|
print(f"[HOMO] Usando triplet: {homo_ref_resolved}")
|
||
|
|
for cam in cams:
|
||
|
|
print(f" {cam}: {homo_triplet[cam]}")
|
||
|
|
|
||
|
|
homo_result = compute_planar_homography_from_triplet(
|
||
|
|
triplet=homo_triplet,
|
||
|
|
cams=cams,
|
||
|
|
cam_roles=cam_roles,
|
||
|
|
sizes=sizes,
|
||
|
|
image_size=image_size,
|
||
|
|
args=args,
|
||
|
|
board=board,
|
||
|
|
aruco_dict=aruco_dict,
|
||
|
|
)
|
||
|
|
|
||
|
|
hs = homo_result["stats"]
|
||
|
|
print("[HOMO] Resultado planar:")
|
||
|
|
print(f" RGB corners={hs['rgb_corners']} RE corners={hs['re_corners']} NIR corners={hs['nir_corners']}")
|
||
|
|
print(f" RE common/inliers={hs['re_common']}/{hs['re_inliers']}")
|
||
|
|
print(f" NIR common/inliers={hs['nir_common']}/{hs['nir_inliers']}")
|
||
|
|
print(f" overlap RE={hs['overlap_re_pct']:.1f}% NIR={hs['overlap_nir_pct']:.1f}% common={hs['overlap_common_pct']:.1f}%")
|
||
|
|
|
||
|
|
out_path = out_dir / f"multicam_calib_{'_'.join(cams)}_ref_{args.ref_cam}.npz"
|
||
|
|
|
||
|
|
save_dict = {
|
||
|
|
"schema": "multicam_charuco_raw10_v2_planar_homography",
|
||
|
|
"cams": np.array(cams),
|
||
|
|
"rgb_cam": args.rgb_cam,
|
||
|
|
"nir_cam": args.nir_cam,
|
||
|
|
"re_cam": args.re_cam,
|
||
|
|
"ref_cam": args.ref_cam,
|
||
|
|
"image_size": np.array(image_size, dtype=np.int32),
|
||
|
|
"squares_x": args.squares_x,
|
||
|
|
"squares_y": args.squares_y,
|
||
|
|
"square_length": args.square_length,
|
||
|
|
"marker_length": args.marker_length,
|
||
|
|
"aruco_dict": args.aruco_dict,
|
||
|
|
"rectify_alpha": args.rectify_alpha,
|
||
|
|
"accepted": accepted,
|
||
|
|
"rejected": rejected,
|
||
|
|
}
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
save_dict[f"role_{cam}"] = cam_roles[cam]
|
||
|
|
save_dict[f"rms_{cam}"] = rms_single[cam]
|
||
|
|
save_dict[f"K_{cam}"] = K[cam]
|
||
|
|
save_dict[f"D_{cam}"] = D[cam]
|
||
|
|
save_dict[f"R_{cam}_to_{args.ref_cam}"] = extr_R_to_ref[cam]
|
||
|
|
save_dict[f"T_{cam}_to_{args.ref_cam}"] = extr_T_to_ref[cam]
|
||
|
|
|
||
|
|
for key, res in pair_results.items():
|
||
|
|
save_dict[f"pair_{key}_cam1"] = res["cam1"]
|
||
|
|
save_dict[f"pair_{key}_cam2"] = res["cam2"]
|
||
|
|
save_dict[f"pair_{key}_rms"] = res["rms"]
|
||
|
|
save_dict[f"pair_{key}_R"] = res["R"]
|
||
|
|
save_dict[f"pair_{key}_T"] = res["T"]
|
||
|
|
save_dict[f"pair_{key}_E"] = res["E"]
|
||
|
|
save_dict[f"pair_{key}_F"] = res["F"]
|
||
|
|
|
||
|
|
rect = res["rect"]
|
||
|
|
for rk, rv in rect.items():
|
||
|
|
save_dict[f"pair_{key}_{rk}"] = rv
|
||
|
|
|
||
|
|
if homo_result is not None:
|
||
|
|
hs = homo_result["stats"]
|
||
|
|
save_dict["has_planar_homography"] = True
|
||
|
|
save_dict["planar_homography_source"] = str(args.homo_ref_frame)
|
||
|
|
save_dict["planar_homography_resolved"] = str(homo_ref_resolved)
|
||
|
|
save_dict["planar_homography_note"] = "Homography maps RE/NIR to RGB for the physical plane visible in homo_ref_frame."
|
||
|
|
|
||
|
|
save_dict["H_RE_to_RGB"] = homo_result["H_RE_to_RGB"]
|
||
|
|
save_dict["H_NIR_to_RGB"] = homo_result["H_NIR_to_RGB"]
|
||
|
|
save_dict[f"H_{args.re_cam}_to_{args.rgb_cam}"] = homo_result["H_RE_to_RGB"]
|
||
|
|
save_dict[f"H_{args.nir_cam}_to_{args.rgb_cam}"] = homo_result["H_NIR_to_RGB"]
|
||
|
|
|
||
|
|
save_dict["homography_mask_RE_to_RGB"] = homo_result["mask_RE_to_RGB"]
|
||
|
|
save_dict["homography_mask_NIR_to_RGB"] = homo_result["mask_NIR_to_RGB"]
|
||
|
|
save_dict["homography_common_ids_RE_to_RGB"] = homo_result["common_ids_RE_to_RGB"]
|
||
|
|
save_dict["homography_common_ids_NIR_to_RGB"] = homo_result["common_ids_NIR_to_RGB"]
|
||
|
|
|
||
|
|
save_dict["overlap_RE_to_RGB"] = homo_result["overlap_RE_to_RGB"]
|
||
|
|
save_dict["overlap_NIR_to_RGB"] = homo_result["overlap_NIR_to_RGB"]
|
||
|
|
save_dict["overlap_common_RGB"] = homo_result["overlap_common_RGB"]
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
save_dict[f"planar_homography_file_{cam}"] = str(homo_triplet[cam])
|
||
|
|
|
||
|
|
for k, v in hs.items():
|
||
|
|
save_dict[f"planar_homography_{k}"] = v
|
||
|
|
else:
|
||
|
|
save_dict["has_planar_homography"] = False
|
||
|
|
|
||
|
|
np.savez_compressed(out_path, **save_dict)
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print(f"[OK] calibração multicâmera salva em: {out_path}")
|
||
|
|
print(f"[OK] debug salvo em: {debug_dir}")
|
||
|
|
print("")
|
||
|
|
print("Resumo individual:")
|
||
|
|
for cam in cams:
|
||
|
|
print(f" {cam:5s} role={cam_roles[cam]:3s} RMS={rms_single[cam]:.6f}")
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print("Resumo pares:")
|
||
|
|
for key, res in pair_results.items():
|
||
|
|
print(f" {key:11s} RMS={res['rms']:.6f} T={res['T'].ravel()}")
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print(f"Extrínsecos para referência {args.ref_cam}:")
|
||
|
|
for cam in cams:
|
||
|
|
print(f" {cam} -> {args.ref_cam}: T={extr_T_to_ref[cam].ravel()}")
|
||
|
|
|
||
|
|
if homo_result is not None:
|
||
|
|
print("")
|
||
|
|
print("Homografia planar salva:")
|
||
|
|
print(f" H_RE_to_RGB ({args.re_cam}->{args.rgb_cam})")
|
||
|
|
print(f" H_NIR_to_RGB ({args.nir_cam}->{args.rgb_cam})")
|
||
|
|
print(f" overlap_common_RGB={homo_result['stats']['overlap_common_pct']:.1f}%")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|