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()