771 lines
22 KiB
Python
771 lines
22 KiB
Python
|
import argparse
|
||
|
|
import json
|
||
|
|
import re
|
||
|
|
from pathlib import Path
|
||
|
|
|
||
|
|
import cv2
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# RAW10
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
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 raw10_bin_to_gray(path: Path, width: int, height: int, use_clahe=True) -> np.ndarray:
|
||
|
|
raw = path.read_bytes()
|
||
|
|
mono10 = unpack_raw10_packed(raw, width, height)
|
||
|
|
gray = normalize_to_u8(mono10)
|
||
|
|
|
||
|
|
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
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def clean_stem_for_pair(path: Path, cam_key: str):
|
||
|
|
stem = path.stem
|
||
|
|
s = 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 pair_cam_bins(root_dir: Path, left_cam: str, right_cam: str):
|
||
|
|
left_bins = find_cam_bins(root_dir, left_cam)
|
||
|
|
right_bins = find_cam_bins(root_dir, right_cam)
|
||
|
|
|
||
|
|
right_by_folder_key = {}
|
||
|
|
right_by_key = {}
|
||
|
|
|
||
|
|
for rp in right_bins:
|
||
|
|
key = clean_stem_for_pair(rp, right_cam)
|
||
|
|
right_by_folder_key[(rp.parent, key)] = rp
|
||
|
|
right_by_key.setdefault(key, rp)
|
||
|
|
|
||
|
|
pairs = []
|
||
|
|
|
||
|
|
for lp in left_bins:
|
||
|
|
key = clean_stem_for_pair(lp, left_cam)
|
||
|
|
|
||
|
|
rp = right_by_folder_key.get((lp.parent, key))
|
||
|
|
if rp is None:
|
||
|
|
rp = right_by_key.get(key)
|
||
|
|
|
||
|
|
if rp is None:
|
||
|
|
same_folder = [r for r in right_bins if r.parent == lp.parent]
|
||
|
|
if len(same_folder) == 1:
|
||
|
|
rp = same_folder[0]
|
||
|
|
|
||
|
|
if rp is not None:
|
||
|
|
pairs.append((lp, rp))
|
||
|
|
|
||
|
|
if pairs:
|
||
|
|
return pairs, left_bins, right_bins, "name/key"
|
||
|
|
|
||
|
|
n = min(len(left_bins), len(right_bins))
|
||
|
|
return list(zip(left_bins[:n], right_bins[:n])), left_bins, right_bins, "order"
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# 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):
|
||
|
|
"""
|
||
|
|
Retorna:
|
||
|
|
charuco_corners: Nx2 float32
|
||
|
|
charuco_ids: N int32
|
||
|
|
marker_corners, marker_ids
|
||
|
|
"""
|
||
|
|
aruco = cv2.aruco
|
||
|
|
params = create_detector_params()
|
||
|
|
|
||
|
|
# OpenCV novo
|
||
|
|
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, marker_corners, marker_ids
|
||
|
|
|
||
|
|
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, marker_corners, marker_ids
|
||
|
|
|
||
|
|
return corners, ids, marker_corners, marker_ids
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
# OpenCV legado
|
||
|
|
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, marker_corners, marker_ids
|
||
|
|
|
||
|
|
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, marker_corners, marker_ids
|
||
|
|
|
||
|
|
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, marker_corners, marker_ids
|
||
|
|
|
||
|
|
return corners, ids, marker_corners, marker_ids
|
||
|
|
|
||
|
|
|
||
|
|
def common_charuco_points(corners_l, ids_l, corners_r, ids_r, board_corners, min_common):
|
||
|
|
map_l = {int(i): corners_l[k] for k, i in enumerate(ids_l)}
|
||
|
|
map_r = {int(i): corners_r[k] for k, i in enumerate(ids_r)}
|
||
|
|
|
||
|
|
common_ids = sorted(set(map_l.keys()) & set(map_r.keys()))
|
||
|
|
|
||
|
|
if len(common_ids) < min_common:
|
||
|
|
return None, None, None, common_ids
|
||
|
|
|
||
|
|
obj = []
|
||
|
|
img_l = []
|
||
|
|
img_r = []
|
||
|
|
|
||
|
|
max_id = len(board_corners) - 1
|
||
|
|
|
||
|
|
for cid in common_ids:
|
||
|
|
if cid < 0 or cid > max_id:
|
||
|
|
continue
|
||
|
|
|
||
|
|
obj.append(board_corners[cid])
|
||
|
|
img_l.append(map_l[cid])
|
||
|
|
img_r.append(map_r[cid])
|
||
|
|
|
||
|
|
if len(obj) < min_common:
|
||
|
|
return None, None, None, common_ids
|
||
|
|
|
||
|
|
obj = np.array(obj, dtype=np.float32).reshape(-1, 1, 3)
|
||
|
|
img_l = np.array(img_l, dtype=np.float32).reshape(-1, 1, 2)
|
||
|
|
img_r = np.array(img_r, dtype=np.float32).reshape(-1, 1, 2)
|
||
|
|
|
||
|
|
return obj, img_l, img_r, common_ids
|
||
|
|
|
||
|
|
|
||
|
|
def draw_debug(gray, charuco_corners, charuco_ids, title):
|
||
|
|
bgr = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
|
||
|
|
|
||
|
|
if charuco_corners is not None and charuco_ids is not None:
|
||
|
|
corners_draw = np.array(charuco_corners, dtype=np.float32).reshape(-1, 1, 2)
|
||
|
|
ids_draw = np.array(charuco_ids, dtype=np.int32).reshape(-1, 1)
|
||
|
|
try:
|
||
|
|
cv2.aruco.drawDetectedCornersCharuco(bgr, corners_draw, ids_draw, (0, 255, 0))
|
||
|
|
except Exception:
|
||
|
|
for p in charuco_corners:
|
||
|
|
cv2.circle(bgr, tuple(np.round(p).astype(int)), 3, (0, 255, 0), -1)
|
||
|
|
|
||
|
|
cv2.putText(
|
||
|
|
bgr,
|
||
|
|
title,
|
||
|
|
(20, 35),
|
||
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||
|
|
0.85,
|
||
|
|
(255, 255, 255),
|
||
|
|
2,
|
||
|
|
cv2.LINE_AA
|
||
|
|
)
|
||
|
|
|
||
|
|
return bgr
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Calibration
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def calibrate_single_camera(objpoints, imgpoints, image_size):
|
||
|
|
flags = 0
|
||
|
|
|
||
|
|
ret, K, D, rvecs, tvecs = cv2.calibrateCamera(
|
||
|
|
objectPoints=objpoints,
|
||
|
|
imagePoints=imgpoints,
|
||
|
|
imageSize=image_size,
|
||
|
|
cameraMatrix=None,
|
||
|
|
distCoeffs=None,
|
||
|
|
flags=flags
|
||
|
|
)
|
||
|
|
|
||
|
|
return ret, K, D, rvecs, tvecs
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
parser = argparse.ArgumentParser()
|
||
|
|
|
||
|
|
parser.add_argument("--root_dir", required=True)
|
||
|
|
parser.add_argument("--out_dir", default="calibration/stereo_charuco_calib_out")
|
||
|
|
|
||
|
|
parser.add_argument("--left_cam", default="CAM_C")
|
||
|
|
parser.add_argument("--right_cam", default="CAM_B")
|
||
|
|
|
||
|
|
parser.add_argument("--width", type=int, default=-1)
|
||
|
|
parser.add_argument("--height", type=int, default=-1)
|
||
|
|
|
||
|
|
# ChArUco 7x13
|
||
|
|
parser.add_argument("--squares_x", type=int, default=7)
|
||
|
|
parser.add_argument("--squares_y", type=int, default=13)
|
||
|
|
|
||
|
|
# Use a unidade que quiser. Recomendo metros.
|
||
|
|
# Ex: quadrado de 20 mm => 0.020
|
||
|
|
parser.add_argument("--square_length", type=float, required=True)
|
||
|
|
parser.add_argument("--marker_length", type=float, required=True)
|
||
|
|
|
||
|
|
parser.add_argument("--aruco_dict", default="4X4_50")
|
||
|
|
|
||
|
|
parser.add_argument("--min_corners", type=int, default=12)
|
||
|
|
parser.add_argument("--min_common", type=int, default=10)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
left_cam = args.left_cam
|
||
|
|
right_cam = args.right_cam
|
||
|
|
|
||
|
|
w_left, h_left = resolve_width_height(args, left_cam)
|
||
|
|
w_right, h_right = resolve_width_height(args, right_cam)
|
||
|
|
|
||
|
|
if (w_left, h_left) != (w_right, h_right):
|
||
|
|
print(f"[WARN] Resoluções diferentes: {left_cam}=({w_left},{h_left}) {right_cam}=({w_right},{h_right})")
|
||
|
|
print("[WARN] Vou calibrar usando o menor tamanho comum após resize.")
|
||
|
|
|
||
|
|
image_w = min(w_left, w_right)
|
||
|
|
image_h = min(h_left, h_right)
|
||
|
|
image_size = (image_w, image_h)
|
||
|
|
|
||
|
|
print(f"[INFO] root_dir={root_dir}")
|
||
|
|
print(f"[INFO] left={left_cam} {w_left}x{h_left}")
|
||
|
|
print(f"[INFO] right={right_cam} {w_right}x{h_right}")
|
||
|
|
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] aruco_dict={args.aruco_dict}")
|
||
|
|
|
||
|
|
pairs, left_bins, right_bins, pair_mode = pair_cam_bins(root_dir, left_cam, right_cam)
|
||
|
|
|
||
|
|
print(f"[INFO] arquivos {left_cam}: {len(left_bins)}")
|
||
|
|
print(f"[INFO] arquivos {right_cam}: {len(right_bins)}")
|
||
|
|
print(f"[INFO] pares encontrados: {len(pairs)}")
|
||
|
|
print(f"[INFO] pareamento: {pair_mode}")
|
||
|
|
|
||
|
|
if len(pairs) < 5:
|
||
|
|
raise RuntimeError("Poucos pares encontrados. Capture mais imagens ou verifique nomes dos arquivos.")
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
stereo_objpoints = []
|
||
|
|
stereo_imgpoints_l = []
|
||
|
|
stereo_imgpoints_r = []
|
||
|
|
|
||
|
|
left_objpoints = []
|
||
|
|
left_imgpoints = []
|
||
|
|
|
||
|
|
right_objpoints = []
|
||
|
|
right_imgpoints = []
|
||
|
|
|
||
|
|
accepted = 0
|
||
|
|
rejected = 0
|
||
|
|
|
||
|
|
for idx, (lp, rp) in enumerate(pairs):
|
||
|
|
print(f"[{idx+1}/{len(pairs)}] L={lp.name} | R={rp.name}")
|
||
|
|
|
||
|
|
try:
|
||
|
|
gray_l = raw10_bin_to_gray(lp, w_left, h_left, use_clahe=not args.no_clahe)
|
||
|
|
gray_r = raw10_bin_to_gray(rp, w_right, h_right, use_clahe=not args.no_clahe)
|
||
|
|
|
||
|
|
if gray_l.shape[::-1] != image_size:
|
||
|
|
gray_l = cv2.resize(gray_l, image_size, interpolation=cv2.INTER_AREA)
|
||
|
|
if gray_r.shape[::-1] != image_size:
|
||
|
|
gray_r = cv2.resize(gray_r, image_size, interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
corners_l, ids_l, _, _ = detect_charuco(
|
||
|
|
gray_l,
|
||
|
|
board,
|
||
|
|
aruco_dict,
|
||
|
|
min_corners=args.min_corners
|
||
|
|
)
|
||
|
|
|
||
|
|
corners_r, ids_r, _, _ = detect_charuco(
|
||
|
|
gray_r,
|
||
|
|
board,
|
||
|
|
aruco_dict,
|
||
|
|
min_corners=args.min_corners
|
||
|
|
)
|
||
|
|
|
||
|
|
n_l = 0 if ids_l is None else len(ids_l)
|
||
|
|
n_r = 0 if ids_r is None else len(ids_r)
|
||
|
|
|
||
|
|
if corners_l is None or corners_r is None:
|
||
|
|
print(f" [REJECT] detect insuficiente: left={n_l}, right={n_r}")
|
||
|
|
rejected += 1
|
||
|
|
continue
|
||
|
|
|
||
|
|
obj, img_l, img_r, common_ids = common_charuco_points(
|
||
|
|
corners_l,
|
||
|
|
ids_l,
|
||
|
|
corners_r,
|
||
|
|
ids_r,
|
||
|
|
board_corners,
|
||
|
|
min_common=args.min_common
|
||
|
|
)
|
||
|
|
|
||
|
|
if obj is None:
|
||
|
|
print(f" [REJECT] comuns insuficientes: common={len(common_ids)}")
|
||
|
|
rejected += 1
|
||
|
|
continue
|
||
|
|
|
||
|
|
stereo_objpoints.append(obj)
|
||
|
|
stereo_imgpoints_l.append(img_l)
|
||
|
|
stereo_imgpoints_r.append(img_r)
|
||
|
|
|
||
|
|
left_objpoints.append(obj.copy())
|
||
|
|
left_imgpoints.append(img_l.copy())
|
||
|
|
|
||
|
|
right_objpoints.append(obj.copy())
|
||
|
|
right_imgpoints.append(img_r.copy())
|
||
|
|
|
||
|
|
accepted += 1
|
||
|
|
|
||
|
|
dbg_l = draw_debug(gray_l, corners_l, ids_l, f"{left_cam} corners={n_l}")
|
||
|
|
dbg_r = draw_debug(gray_r, corners_r, ids_r, f"{right_cam} corners={n_r}")
|
||
|
|
dbg = np.hstack([dbg_l, dbg_r])
|
||
|
|
|
||
|
|
cv2.putText(
|
||
|
|
dbg,
|
||
|
|
f"COMMON={len(common_ids)} ACCEPTED",
|
||
|
|
(20, dbg.shape[0] - 25),
|
||
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||
|
|
0.9,
|
||
|
|
(0, 255, 0),
|
||
|
|
2,
|
||
|
|
cv2.LINE_AA
|
||
|
|
)
|
||
|
|
|
||
|
|
cv2.imwrite(str(debug_dir / f"pair_{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(f" [OK] left={n_l}, right={n_r}, common={len(common_ids)}")
|
||
|
|
|
||
|
|
except Exception as e:
|
||
|
|
print(f" [ERRO] {e}")
|
||
|
|
rejected += 1
|
||
|
|
|
||
|
|
if args.show:
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print(f"[INFO] aceitos: {accepted}")
|
||
|
|
print(f"[INFO] rejeitados: {rejected}")
|
||
|
|
|
||
|
|
if accepted < 8:
|
||
|
|
raise RuntimeError(
|
||
|
|
f"Poucos pares aceitos: {accepted}. Ideal: pelo menos 15-25 bons, melhor 30+."
|
||
|
|
)
|
||
|
|
|
||
|
|
print("[CALIB] Calibrando câmera esquerda...")
|
||
|
|
ret_l, K_l, D_l, rvecs_l, tvecs_l = calibrate_single_camera(
|
||
|
|
left_objpoints,
|
||
|
|
left_imgpoints,
|
||
|
|
image_size
|
||
|
|
)
|
||
|
|
|
||
|
|
print("[CALIB] Calibrando câmera direita...")
|
||
|
|
ret_r, K_r, D_r, rvecs_r, tvecs_r = calibrate_single_camera(
|
||
|
|
right_objpoints,
|
||
|
|
right_imgpoints,
|
||
|
|
image_size
|
||
|
|
)
|
||
|
|
|
||
|
|
print(f"[RESULT] RMS left : {ret_l:.6f}")
|
||
|
|
print(f"[RESULT] RMS right: {ret_r:.6f}")
|
||
|
|
|
||
|
|
print("[CALIB] Calibração estéreo...")
|
||
|
|
|
||
|
|
stereo_flags = cv2.CALIB_FIX_INTRINSIC
|
||
|
|
|
||
|
|
ret_stereo, K_l2, D_l2, K_r2, D_r2, R, T, E, F = cv2.stereoCalibrate(
|
||
|
|
objectPoints=stereo_objpoints,
|
||
|
|
imagePoints1=stereo_imgpoints_l,
|
||
|
|
imagePoints2=stereo_imgpoints_r,
|
||
|
|
cameraMatrix1=K_l,
|
||
|
|
distCoeffs1=D_l,
|
||
|
|
cameraMatrix2=K_r,
|
||
|
|
distCoeffs2=D_r,
|
||
|
|
imageSize=image_size,
|
||
|
|
flags=stereo_flags,
|
||
|
|
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 200, 1e-7)
|
||
|
|
)
|
||
|
|
|
||
|
|
print(f"[RESULT] RMS stereo: {ret_stereo:.6f}")
|
||
|
|
print(f"[RESULT] T: {T.ravel()}")
|
||
|
|
|
||
|
|
print("[CALIB] stereoRectify...")
|
||
|
|
|
||
|
|
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
|
||
|
|
cameraMatrix1=K_l2,
|
||
|
|
distCoeffs1=D_l2,
|
||
|
|
cameraMatrix2=K_r2,
|
||
|
|
distCoeffs2=D_r2,
|
||
|
|
imageSize=image_size,
|
||
|
|
R=R,
|
||
|
|
T=T,
|
||
|
|
flags=cv2.CALIB_ZERO_DISPARITY,
|
||
|
|
alpha=0
|
||
|
|
)
|
||
|
|
|
||
|
|
map1x, map1y = cv2.initUndistortRectifyMap(
|
||
|
|
K_l2,
|
||
|
|
D_l2,
|
||
|
|
R1,
|
||
|
|
P1,
|
||
|
|
image_size,
|
||
|
|
cv2.CV_32FC1
|
||
|
|
)
|
||
|
|
|
||
|
|
map2x, map2y = cv2.initUndistortRectifyMap(
|
||
|
|
K_r2,
|
||
|
|
D_r2,
|
||
|
|
R2,
|
||
|
|
P2,
|
||
|
|
image_size,
|
||
|
|
cv2.CV_32FC1
|
||
|
|
)
|
||
|
|
|
||
|
|
out_path = out_dir / f"stereo_calib_{left_cam}_{right_cam}.npz"
|
||
|
|
|
||
|
|
np.savez_compressed(
|
||
|
|
out_path,
|
||
|
|
left_cam=left_cam,
|
||
|
|
right_cam=right_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,
|
||
|
|
|
||
|
|
rms_left=ret_l,
|
||
|
|
rms_right=ret_r,
|
||
|
|
rms_stereo=ret_stereo,
|
||
|
|
|
||
|
|
K_left=K_l2,
|
||
|
|
D_left=D_l2,
|
||
|
|
K_right=K_r2,
|
||
|
|
D_right=D_r2,
|
||
|
|
|
||
|
|
R=R,
|
||
|
|
T=T,
|
||
|
|
E=E,
|
||
|
|
F=F,
|
||
|
|
|
||
|
|
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,
|
||
|
|
|
||
|
|
accepted=accepted,
|
||
|
|
rejected=rejected
|
||
|
|
)
|
||
|
|
|
||
|
|
print("")
|
||
|
|
print(f"[OK] calibração salva em: {out_path}")
|
||
|
|
print(f"[OK] debug salvo em: {debug_dir}")
|
||
|
|
print("")
|
||
|
|
print("Resumo:")
|
||
|
|
print(f" RMS left = {ret_l:.6f}")
|
||
|
|
print(f" RMS right = {ret_r:.6f}")
|
||
|
|
print(f" RMS stereo = {ret_stereo:.6f}")
|
||
|
|
print(f" T = {T.ravel()}")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|