1502 lines
53 KiB
Python
1502 lines
53 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)
|
|
|
|
# Para detecção ChArUco, usar o RAW Bayer como intensidade costuma ser mais fiel
|
|
# que debayerizar, porque o debayer pode suavizar os IDs ArUco.
|
|
# O parâmetro is_rgb fica mantido por compatibilidade com chamadas antigas.
|
|
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")) +
|
|
sorted(root_dir.rglob("*.json"))
|
|
)
|
|
return candidates[0] if candidates else None
|
|
|
|
|
|
def extract_camera_info_from_meta(meta: dict, cam_key: str):
|
|
# Caminho usado pelo meta atual do oak_fcc3.
|
|
stream_meta = meta.get("stream_meta")
|
|
if isinstance(stream_meta, dict):
|
|
camera_info = stream_meta.get("camera_info")
|
|
if isinstance(camera_info, dict):
|
|
info = camera_info.get(cam_key)
|
|
if isinstance(info, dict) and "width" in info and "height" in info:
|
|
return info
|
|
|
|
# Formatos alternativos.
|
|
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) and "width" in info and "height" in info:
|
|
return info
|
|
|
|
# Busca recursiva, mas só aceita se parecer info geométrica da câmera.
|
|
stack = [meta]
|
|
while stack:
|
|
obj = stack.pop()
|
|
|
|
if isinstance(obj, dict):
|
|
if cam_key in obj and isinstance(obj[cam_key], dict):
|
|
info = obj[cam_key]
|
|
if "width" in info and "height" in info:
|
|
return info
|
|
|
|
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, search_root: Path):
|
|
if args.width > 0 and args.height > 0:
|
|
return args.width, args.height
|
|
|
|
w, h = try_get_width_height_from_meta(search_root, 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}")
|
|
|
|
|
|
def find_triplets_multi(root_dirs: list[Path], cams: list[str]):
|
|
all_triplets = []
|
|
all_by_cam = {cam: [] for cam in cams}
|
|
|
|
for root in root_dirs:
|
|
triplets, by_cam = find_triplets(root, cams)
|
|
|
|
for cam in cams:
|
|
all_by_cam[cam].extend(by_cam[cam])
|
|
|
|
for item in triplets:
|
|
item = dict(item)
|
|
item["__root_dir"] = root
|
|
all_triplets.append(item)
|
|
|
|
print(f"[INFO] root_dir={root} triplets={len(triplets)}")
|
|
|
|
return all_triplets, all_by_cam
|
|
|
|
|
|
# ============================================================
|
|
# 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"):
|
|
params = aruco.DetectorParameters()
|
|
elif hasattr(aruco, "DetectorParameters_create"):
|
|
params = aruco.DetectorParameters_create()
|
|
else:
|
|
return None
|
|
|
|
params.adaptiveThreshWinSizeMin = 3
|
|
params.adaptiveThreshWinSizeMax = 53
|
|
params.adaptiveThreshWinSizeStep = 4
|
|
|
|
params.minMarkerPerimeterRate = 0.01
|
|
params.maxMarkerPerimeterRate = 4.0
|
|
|
|
params.polygonalApproxAccuracyRate = 0.05
|
|
params.minCornerDistanceRate = 0.02
|
|
params.minDistanceToBorder = 1
|
|
|
|
try:
|
|
params.cornerRefinementMethod = aruco.CORNER_REFINE_SUBPIX
|
|
params.cornerRefinementWinSize = 5
|
|
params.cornerRefinementMaxIterations = 50
|
|
params.cornerRefinementMinAccuracy = 0.01
|
|
except Exception:
|
|
pass
|
|
|
|
return params
|
|
|
|
|
|
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,
|
|
}
|
|
|
|
|
|
|
|
def detect_charuco_best(gray: np.ndarray, board, aruco_dict, min_corners: int, cam: str = ""):
|
|
"""
|
|
Tenta múltiplos pré-processamentos e escalas.
|
|
Retorna a melhor detecção mesmo quando ela fica abaixo de min_corners.
|
|
"""
|
|
variants = []
|
|
|
|
base = gray.copy()
|
|
variants.append(("raw", base))
|
|
|
|
clahe2 = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
|
clahe4 = cv2.createCLAHE(clipLimit=4.0, tileGridSize=(8, 8))
|
|
|
|
variants.append(("clahe_2", clahe2.apply(base)))
|
|
variants.append(("clahe_4", clahe4.apply(base)))
|
|
|
|
blur = cv2.GaussianBlur(base, (3, 3), 0)
|
|
variants.append(("blur_clahe_2", clahe2.apply(blur)))
|
|
|
|
th = cv2.adaptiveThreshold(
|
|
base,
|
|
255,
|
|
cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
|
|
cv2.THRESH_BINARY,
|
|
31,
|
|
5,
|
|
)
|
|
variants.append(("adaptive", th))
|
|
|
|
variants.append(("invert_raw", 255 - base))
|
|
variants.append(("invert_clahe_2", 255 - clahe2.apply(base)))
|
|
|
|
best = {
|
|
"name": None,
|
|
"corners": None,
|
|
"ids": None,
|
|
"count": 0,
|
|
"accepted": False,
|
|
}
|
|
|
|
for name, img in variants:
|
|
for scale in [1.0, 2.0, 3.0]:
|
|
if scale == 1.0:
|
|
test_img = img
|
|
else:
|
|
test_img = cv2.resize(
|
|
img,
|
|
None,
|
|
fx=scale,
|
|
fy=scale,
|
|
interpolation=cv2.INTER_CUBIC,
|
|
)
|
|
|
|
corners, ids = detect_charuco(test_img, board, aruco_dict, min_corners=1)
|
|
count = 0 if ids is None else len(ids)
|
|
|
|
if count > best["count"]:
|
|
if corners is not None and scale != 1.0:
|
|
corners = corners / scale
|
|
|
|
best.update({
|
|
"name": f"{name}_x{scale:g}" if scale != 1.0 else name,
|
|
"corners": corners,
|
|
"ids": ids,
|
|
"count": count,
|
|
"accepted": count >= min_corners,
|
|
})
|
|
|
|
return best["corners"], best["ids"], best["name"], best["count"], best["accepted"]
|
|
|
|
|
|
def _make_overlap_masks_from_homographies(H_re, H_nir, image_size: tuple[int, int]):
|
|
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,
|
|
)
|
|
|
|
overlap_common_rgb = cv2.bitwise_and(overlap_re_to_rgb, overlap_nir_to_rgb)
|
|
return overlap_re_to_rgb, overlap_nir_to_rgb, overlap_common_rgb
|
|
|
|
|
|
def compute_planar_homography_from_collected_pairs(
|
|
homography_pairs: dict,
|
|
image_size: tuple[int, int],
|
|
args,
|
|
):
|
|
"""
|
|
Calcula H_RE_to_RGB e H_NIR_to_RGB usando TODOS os pares válidos acumulados
|
|
durante a varredura do dataset.
|
|
|
|
Premissa: todos os frames usados representam o mesmo plano físico.
|
|
"""
|
|
re_items = homography_pairs.get("RE_to_RGB", [])
|
|
nir_items = homography_pairs.get("NIR_to_RGB", [])
|
|
|
|
if len(re_items) == 0:
|
|
raise RuntimeError("Nenhum par válido acumulado para homografia RE -> RGB.")
|
|
|
|
if len(nir_items) == 0:
|
|
raise RuntimeError("Nenhum par válido acumulado para homografia NIR -> RGB.")
|
|
|
|
def stack_points(items, label):
|
|
src = np.vstack([x["pts_src"] for x in items]).astype(np.float32)
|
|
dst = np.vstack([x["pts_dst"] for x in items]).astype(np.float32)
|
|
|
|
min_total = max(4, int(args.homo_min_total_points))
|
|
if len(src) < min_total:
|
|
raise RuntimeError(
|
|
f"Poucos pontos totais para homografia {label}: {len(src)}. "
|
|
f"Mínimo configurado={min_total}."
|
|
)
|
|
|
|
return src, dst
|
|
|
|
pts_re_src, pts_re_dst = stack_points(re_items, "RE_to_RGB")
|
|
pts_nir_src, pts_nir_dst = stack_points(nir_items, "NIR_to_RGB")
|
|
|
|
H_re, mask_re = cv2.findHomography(
|
|
pts_re_src,
|
|
pts_re_dst,
|
|
cv2.RANSAC,
|
|
args.homo_ransac_thresh,
|
|
)
|
|
|
|
H_nir, mask_nir = cv2.findHomography(
|
|
pts_nir_src,
|
|
pts_nir_dst,
|
|
cv2.RANSAC,
|
|
args.homo_ransac_thresh,
|
|
)
|
|
|
|
if H_re is None:
|
|
raise RuntimeError("cv2.findHomography falhou para RE -> RGB usando todos os frames válidos.")
|
|
|
|
if H_nir is None:
|
|
raise RuntimeError("cv2.findHomography falhou para NIR -> RGB usando todos os frames válidos.")
|
|
|
|
re_inliers = int(np.count_nonzero(mask_re)) if mask_re is not None else 0
|
|
nir_inliers = int(np.count_nonzero(mask_nir)) if mask_nir is not None else 0
|
|
|
|
overlap_re_to_rgb, overlap_nir_to_rgb, overlap_common_rgb = _make_overlap_masks_from_homographies(
|
|
H_re,
|
|
H_nir,
|
|
image_size,
|
|
)
|
|
|
|
re_frame_indices = sorted({int(x["triplet_idx"]) for x in re_items})
|
|
nir_frame_indices = sorted({int(x["triplet_idx"]) for x in nir_items})
|
|
common_frame_indices = sorted(set(re_frame_indices) & set(nir_frame_indices))
|
|
|
|
stats = {
|
|
"mode": "all_valid_triplets",
|
|
"min_common_frame": int(args.homo_min_common_frame),
|
|
"min_total_points": int(args.homo_min_total_points),
|
|
"re_total_points": int(len(pts_re_src)),
|
|
"nir_total_points": int(len(pts_nir_src)),
|
|
"re_inliers": re_inliers,
|
|
"nir_inliers": nir_inliers,
|
|
"re_inlier_pct": float(re_inliers / max(1, len(pts_re_src)) * 100.0),
|
|
"nir_inlier_pct": float(nir_inliers / max(1, len(pts_nir_src)) * 100.0),
|
|
"re_frames_used": int(len(re_frame_indices)),
|
|
"nir_frames_used": int(len(nir_frame_indices)),
|
|
"common_frames_used": int(len(common_frame_indices)),
|
|
"re_pair_records": int(len(re_items)),
|
|
"nir_pair_records": int(len(nir_items)),
|
|
"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(overlap_common_rgb > 0) * 100.0),
|
|
}
|
|
|
|
common_ids_re = np.concatenate([x["common_ids"] for x in re_items]).astype(np.int32)
|
|
common_ids_nir = np.concatenate([x["common_ids"] for x in nir_items]).astype(np.int32)
|
|
|
|
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": common_ids_re,
|
|
"common_ids_NIR_to_RGB": common_ids_nir,
|
|
"overlap_RE_to_RGB": overlap_re_to_rgb,
|
|
"overlap_NIR_to_RGB": overlap_nir_to_rgb,
|
|
"overlap_common_RGB": overlap_common_rgb,
|
|
"stats": stats,
|
|
"frame_indices_RE_to_RGB": np.array(re_frame_indices, dtype=np.int32),
|
|
"frame_indices_NIR_to_RGB": np.array(nir_frame_indices, dtype=np.int32),
|
|
"frame_indices_common": np.array(common_frame_indices, dtype=np.int32),
|
|
}
|
|
|
|
|
|
def add_homography_to_save_dict(save_dict: dict, homo_result: dict, args):
|
|
hs = homo_result["stats"]
|
|
|
|
save_dict["has_planar_homography"] = True
|
|
save_dict["planar_homography_source"] = "all_valid_triplets"
|
|
save_dict["planar_homography_resolved"] = "all_valid_triplets"
|
|
save_dict["planar_homography_note"] = (
|
|
"Homography maps RE/NIR to RGB using all valid ChArUco detections "
|
|
"from the same physical plane."
|
|
)
|
|
|
|
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["homography_frame_indices_RE_to_RGB"] = homo_result["frame_indices_RE_to_RGB"]
|
|
save_dict["homography_frame_indices_NIR_to_RGB"] = homo_result["frame_indices_NIR_to_RGB"]
|
|
save_dict["homography_frame_indices_common"] = homo_result["frame_indices_common"]
|
|
|
|
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 k, v in hs.items():
|
|
save_dict[f"planar_homography_{k}"] = v
|
|
|
|
return save_dict
|
|
|
|
|
|
# ============================================================
|
|
# Main
|
|
# ============================================================
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("--root_dir", default=None)
|
|
parser.add_argument(
|
|
"--root_dirs",
|
|
nargs="+",
|
|
default=None,
|
|
help="Lista de diretórios de calibração para juntar no mesmo cálculo stereo. Ex: baixa media alta",
|
|
)
|
|
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=0)
|
|
parser.add_argument("--height", type=int, default=0)
|
|
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 usando todos os frames válidos do mesmo plano físico.
|
|
parser.add_argument(
|
|
"--homography_mode",
|
|
default="all_valid",
|
|
choices=["all_valid", "off"],
|
|
help=(
|
|
"Modo de homografia planar. 'all_valid' acumula todos os pares válidos "
|
|
"RE->RGB e NIR->RGB encontrados no dataset. 'off' desativa."
|
|
),
|
|
)
|
|
# Homografia all_valid:
|
|
# - Não corta detecções fracas por câmera antes de acumular pontos.
|
|
# - Cada frame/par contribui se tiver pelo menos homo_min_common_frame IDs comuns.
|
|
# - O corte forte acontece no acumulado total, em homo_min_total_points.
|
|
parser.add_argument(
|
|
"--homo_min_common_frame",
|
|
type=int,
|
|
default=4,
|
|
help=(
|
|
"Mínimo de IDs comuns por frame/par para adicionar pontos à homografia. "
|
|
"Use 4 como mínimo matemático; 5-8 para ficar menos permissivo."
|
|
),
|
|
)
|
|
parser.add_argument(
|
|
"--homo_min_total_points",
|
|
type=int,
|
|
default=30,
|
|
help=(
|
|
"Mínimo de pontos acumulados no dataset inteiro para calcular cada homografia. "
|
|
"Ex: 30 para teste, 50-100 para calibração mais robusta."
|
|
),
|
|
)
|
|
parser.add_argument("--homo_ransac_thresh", type=float, default=3.0)
|
|
|
|
# Compatibilidade com comandos antigos. Não são mais usados como corte da homografia all_valid.
|
|
parser.add_argument("--homo_min_corners", type=int, default=None, help=argparse.SUPPRESS)
|
|
parser.add_argument("--homo_min_common", type=int, default=None, help=argparse.SUPPRESS)
|
|
parser.add_argument(
|
|
"--min_calib_triplets",
|
|
type=int,
|
|
default=5,
|
|
help="Mínimo de triplets aceitos para executar calibração intrínseca/stereo.",
|
|
)
|
|
|
|
parser.add_argument("--no_clahe", action="store_true")
|
|
parser.add_argument("--show", action="store_true")
|
|
|
|
args = parser.parse_args()
|
|
|
|
if args.root_dirs:
|
|
root_dirs = [Path(p) for p in args.root_dirs]
|
|
elif args.root_dir:
|
|
root_dirs = [Path(args.root_dir)]
|
|
else:
|
|
raise RuntimeError("Informe --root_dir ou --root_dirs.")
|
|
|
|
# Compatibilidade com comandos antigos:
|
|
# --homo_min_common antigo vira o novo corte mínimo por frame/par.
|
|
# --homo_min_corners antigo não é mais usado como corte para homografia all_valid,
|
|
# porque agora aceitamos detecções pequenas e filtramos pelo total acumulado.
|
|
if args.homo_min_common is not None:
|
|
args.homo_min_common_frame = int(args.homo_min_common)
|
|
|
|
if getattr(args, "root_dirs", None):
|
|
root_dirs = [Path(p) for p in args.root_dirs]
|
|
elif getattr(args, "root_dir", None):
|
|
root_dirs = [Path(args.root_dir)]
|
|
else:
|
|
raise RuntimeError("Informe --root_dir ou --root_dirs.")
|
|
|
|
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, root_dirs[0])
|
|
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_dirs}")
|
|
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}")
|
|
print(f"[INFO] homography_mode={args.homography_mode}")
|
|
if args.homography_mode == "all_valid":
|
|
print(f"[INFO] homo_min_common_frame={args.homo_min_common_frame}")
|
|
print(f"[INFO] homo_min_total_points={args.homo_min_total_points}")
|
|
print(f"[INFO] homo_ransac_thresh={args.homo_ransac_thresh}")
|
|
|
|
triplets, by_cam = find_triplets_multi(root_dirs, cams)
|
|
for cam in cams:
|
|
print(f"[INFO] arquivos {cam}: {len(by_cam[cam])}")
|
|
print(f"[INFO] triplets encontrados: {len(triplets)}")
|
|
|
|
if len(triplets) < 5:
|
|
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
|
|
|
|
homography_pairs = {
|
|
"RE_to_RGB": [],
|
|
"NIR_to_RGB": [],
|
|
}
|
|
|
|
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, det_mode, det_count, det_ok = detect_charuco_best(
|
|
gray,
|
|
board,
|
|
aruco_dict,
|
|
min_corners=args.min_corners,
|
|
cam=cam,
|
|
)
|
|
|
|
# Mantém a melhor detecção bruta para homografia, mesmo quando
|
|
# ela fica abaixo do mínimo mais rígido da calibração stereo.
|
|
detections[cam] = (corners, ids)
|
|
detections[f"{cam}__count"] = det_count
|
|
detections[f"{cam}__mode"] = det_mode
|
|
|
|
print(
|
|
f" [DETECT] {cam}: best={det_mode} corners={det_count} "
|
|
f"{'OK' if det_ok else f'LOW<{args.min_corners}'}"
|
|
)
|
|
|
|
raw_counts = {cam: (0 if detections[cam][1] is None else len(detections[cam][1])) for cam in cams}
|
|
|
|
# Homografia all_valid:
|
|
# Aqui não usamos corte por câmera tipo "RGB precisa ter 20/40 pontos".
|
|
# Se um frame achou poucos pontos, mas tem pelo menos 4 IDs comuns no par,
|
|
# esses pontos entram no acumulado. O corte forte é feito depois, no total.
|
|
if args.homography_mode == "all_valid":
|
|
rgb_corners, rgb_ids = detections[args.rgb_cam]
|
|
re_corners, re_ids = detections[args.re_cam]
|
|
nir_corners, nir_ids = detections[args.nir_cam]
|
|
|
|
pts_re, pts_rgb_re, common_re = common_points_pair(
|
|
re_corners,
|
|
re_ids,
|
|
rgb_corners,
|
|
rgb_ids,
|
|
min_common=args.homo_min_common_frame,
|
|
)
|
|
if pts_re is not None:
|
|
homography_pairs["RE_to_RGB"].append({
|
|
"triplet_idx": idx,
|
|
"pts_src": pts_re,
|
|
"pts_dst": pts_rgb_re,
|
|
"common_ids": np.array(common_re, dtype=np.int32),
|
|
"src_file": str(item[args.re_cam]),
|
|
"dst_file": str(item[args.rgb_cam]),
|
|
"src_count": raw_counts[args.re_cam],
|
|
"dst_count": raw_counts[args.rgb_cam],
|
|
})
|
|
print(f" [HOMO ADD] RE->RGB common={len(common_re)} total_records={len(homography_pairs['RE_to_RGB'])}")
|
|
else:
|
|
print(f" [HOMO SKIP] RE->RGB common={len(common_re)} < {args.homo_min_common_frame}")
|
|
|
|
pts_nir, pts_rgb_nir, common_nir = common_points_pair(
|
|
nir_corners,
|
|
nir_ids,
|
|
rgb_corners,
|
|
rgb_ids,
|
|
min_common=args.homo_min_common_frame,
|
|
)
|
|
if pts_nir is not None:
|
|
homography_pairs["NIR_to_RGB"].append({
|
|
"triplet_idx": idx,
|
|
"pts_src": pts_nir,
|
|
"pts_dst": pts_rgb_nir,
|
|
"common_ids": np.array(common_nir, dtype=np.int32),
|
|
"src_file": str(item[args.nir_cam]),
|
|
"dst_file": str(item[args.rgb_cam]),
|
|
"src_count": raw_counts[args.nir_cam],
|
|
"dst_count": raw_counts[args.rgb_cam],
|
|
})
|
|
print(f" [HOMO ADD] NIR->RGB common={len(common_nir)} total_records={len(homography_pairs['NIR_to_RGB'])}")
|
|
else:
|
|
print(f" [HOMO SKIP] NIR->RGB common={len(common_nir)} < {args.homo_min_common_frame}")
|
|
|
|
detections_calib = {}
|
|
for cam in cams:
|
|
corners, ids = detections[cam]
|
|
if raw_counts[cam] >= args.min_corners:
|
|
detections_calib[cam] = (corners, ids)
|
|
else:
|
|
detections_calib[cam] = (None, None)
|
|
|
|
counts = {cam: (0 if detections_calib[cam][1] is None else len(detections_calib[cam][1])) for cam in cams}
|
|
|
|
if any(detections_calib[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_calib, 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_calib, 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_calib, 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_calib, 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 para calibração stereo: {accepted}")
|
|
print(f"[INFO] triplets rejeitados para calibração stereo: {rejected}")
|
|
re_acc_points = sum(len(x["pts_src"]) for x in homography_pairs["RE_to_RGB"])
|
|
nir_acc_points = sum(len(x["pts_src"]) for x in homography_pairs["NIR_to_RGB"])
|
|
print(f"[INFO] pares acumulados homografia RE->RGB: {len(homography_pairs['RE_to_RGB'])} | pontos={re_acc_points}")
|
|
print(f"[INFO] pares acumulados homografia NIR->RGB: {len(homography_pairs['NIR_to_RGB'])} | pontos={nir_acc_points}")
|
|
|
|
homo_result = None
|
|
if args.homography_mode == "all_valid":
|
|
print("")
|
|
print("[HOMO] Calculando homografia planar com TODOS os pares válidos do dataset...")
|
|
try:
|
|
homo_result = compute_planar_homography_from_collected_pairs(
|
|
homography_pairs=homography_pairs,
|
|
image_size=image_size,
|
|
args=args,
|
|
)
|
|
|
|
hs = homo_result["stats"]
|
|
print("[HOMO] Resultado planar all_valid:")
|
|
print(f" RE points/inliers={hs['re_total_points']}/{hs['re_inliers']} ({hs['re_inlier_pct']:.1f}%)")
|
|
print(f" NIR points/inliers={hs['nir_total_points']}/{hs['nir_inliers']} ({hs['nir_inlier_pct']:.1f}%)")
|
|
print(f" frames RE/NIR/common={hs['re_frames_used']}/{hs['nir_frames_used']}/{hs['common_frames_used']}")
|
|
print(f" overlap RE={hs['overlap_re_pct']:.1f}% NIR={hs['overlap_nir_pct']:.1f}% common={hs['overlap_common_pct']:.1f}%")
|
|
|
|
except Exception as e:
|
|
print(f"[HOMO][WARN] Não foi possível calcular homografia all_valid: {e}")
|
|
homo_result = None
|
|
|
|
out_path = out_dir / f"multicam_calib_{'_'.join(cams)}_ref_{args.ref_cam}.npz"
|
|
|
|
if accepted < args.min_calib_triplets:
|
|
if homo_result is None:
|
|
raise RuntimeError(
|
|
f"Poucos triplets aceitos para calibração stereo: {accepted}. "
|
|
f"Mínimo configurado={args.min_calib_triplets}. "
|
|
f"Também não foi possível salvar homografia."
|
|
)
|
|
|
|
save_dict = {
|
|
"schema": "multicam_charuco_raw10_v4_planar_homography_accumulated_only",
|
|
"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,
|
|
"stereo_calibration_available": False,
|
|
"stereo_calibration_note": (
|
|
f"Calibração stereo não executada porque accepted={accepted} "
|
|
f"< min_calib_triplets={args.min_calib_triplets}."
|
|
),
|
|
}
|
|
|
|
for cam in cams:
|
|
save_dict[f"role_{cam}"] = cam_roles[cam]
|
|
|
|
add_homography_to_save_dict(save_dict, homo_result, args)
|
|
np.savez_compressed(out_path, **save_dict)
|
|
|
|
print("")
|
|
print(f"[OK] homografia planar salva em: {out_path}")
|
|
print("[OK] calibração stereo não foi executada por falta de triplets aceitos.")
|
|
print(f"[OK] debug salvo em: {debug_dir}")
|
|
return
|
|
|
|
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 all_valid já foi calculada antes da calibração stereo.
|
|
|
|
out_path = out_dir / f"multicam_calib_{'_'.join(cams)}_ref_{args.ref_cam}.npz"
|
|
|
|
save_dict = {
|
|
"schema": "multicam_charuco_raw10_v4_planar_homography_accumulated",
|
|
"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,
|
|
"stereo_calibration_available": True,
|
|
"calibration_mode": "stereo_global",
|
|
"source_root_dirs": np.array([str(p) for p in root_dirs]),
|
|
"source_root_count": len(root_dirs),
|
|
}
|
|
|
|
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:
|
|
add_homography_to_save_dict(save_dict, homo_result, args)
|
|
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()
|