787 lines
24 KiB
Python
787 lines
24 KiB
Python
|
import argparse
|
||
|
|
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:
|
||
|
|
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 read_raw10_rgb_bgr(path: Path, width: int, height: int, bayer: str) -> np.ndarray:
|
||
|
|
raw = path.read_bytes()
|
||
|
|
raw10 = unpack_raw10_packed(raw, width, height)
|
||
|
|
return debayer_raw10_to_bgr_u8(raw10, bayer=bayer)
|
||
|
|
|
||
|
|
|
||
|
|
def read_raw10_rgb_view_bgr(
|
||
|
|
path: Path,
|
||
|
|
width: int,
|
||
|
|
height: int,
|
||
|
|
bayer: str,
|
||
|
|
rgb_view: str,
|
||
|
|
use_clahe: bool = True,
|
||
|
|
) -> np.ndarray:
|
||
|
|
"""
|
||
|
|
Carrega CAM_A/RGB em três modos:
|
||
|
|
|
||
|
|
color:
|
||
|
|
RAW10 Bayer -> debayer BGR -> visual colorido.
|
||
|
|
|
||
|
|
gray:
|
||
|
|
RAW10 Bayer -> debayer BGR -> grayscale -> CLAHE -> BGR fake.
|
||
|
|
Este é o mais parecido com o caminho usado na calibração ChArUco.
|
||
|
|
|
||
|
|
raw_bayer_gray:
|
||
|
|
RAW10 Bayer -> normalize direto -> CLAHE -> BGR fake.
|
||
|
|
Não faz debayer; útil para testar se a interpolação do debayer está influenciando.
|
||
|
|
"""
|
||
|
|
raw = path.read_bytes()
|
||
|
|
raw10 = unpack_raw10_packed(raw, width, height)
|
||
|
|
mode = rgb_view.lower().strip()
|
||
|
|
|
||
|
|
if mode == "color":
|
||
|
|
return debayer_raw10_to_bgr_u8(raw10, bayer=bayer)
|
||
|
|
|
||
|
|
if mode == "gray":
|
||
|
|
bgr = debayer_raw10_to_bgr_u8(raw10, bayer=bayer)
|
||
|
|
gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
|
||
|
|
|
||
|
|
elif mode == "raw_bayer_gray":
|
||
|
|
gray = normalize_to_u8(raw10)
|
||
|
|
|
||
|
|
else:
|
||
|
|
raise RuntimeError(
|
||
|
|
f"rgb_view inválido: {rgb_view}. Use: color, gray ou raw_bayer_gray"
|
||
|
|
)
|
||
|
|
|
||
|
|
if use_clahe:
|
||
|
|
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||
|
|
gray = clahe.apply(gray)
|
||
|
|
|
||
|
|
return cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
|
||
|
|
|
||
|
|
|
||
|
|
def read_raw10_mono_bgr(path: Path, width: int, height: int, use_clahe=True) -> np.ndarray:
|
||
|
|
raw = path.read_bytes()
|
||
|
|
raw10 = unpack_raw10_packed(raw, width, height)
|
||
|
|
gray = normalize_to_u8(raw10)
|
||
|
|
|
||
|
|
if use_clahe:
|
||
|
|
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||
|
|
gray = clahe.apply(gray)
|
||
|
|
|
||
|
|
return cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
|
||
|
|
|
||
|
|
|
||
|
|
def to_gray_u8(img_bgr: np.ndarray) -> np.ndarray:
|
||
|
|
if img_bgr.ndim == 2:
|
||
|
|
return img_bgr
|
||
|
|
return cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Triplet pairing
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
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
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Calibration loading
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def scalar_str(x):
|
||
|
|
arr = np.array(x)
|
||
|
|
if arr.shape == ():
|
||
|
|
return str(arr.item())
|
||
|
|
return str(x)
|
||
|
|
|
||
|
|
|
||
|
|
def load_multicam_calib(calib_path: Path):
|
||
|
|
data = np.load(str(calib_path), allow_pickle=True)
|
||
|
|
keys = set(data.files)
|
||
|
|
|
||
|
|
required = ["image_size", "rgb_cam", "nir_cam", "re_cam", "ref_cam"]
|
||
|
|
for k in required:
|
||
|
|
if k not in keys:
|
||
|
|
raise RuntimeError(f"Calibração multicam sem chave obrigatória: {k}")
|
||
|
|
|
||
|
|
calib = {
|
||
|
|
"data": data,
|
||
|
|
"keys": keys,
|
||
|
|
"image_size": tuple(data["image_size"].astype(int).tolist()),
|
||
|
|
"rgb_cam": scalar_str(data["rgb_cam"]),
|
||
|
|
"nir_cam": scalar_str(data["nir_cam"]),
|
||
|
|
"re_cam": scalar_str(data["re_cam"]),
|
||
|
|
"ref_cam": scalar_str(data["ref_cam"]),
|
||
|
|
}
|
||
|
|
|
||
|
|
return calib
|
||
|
|
|
||
|
|
|
||
|
|
def get_pair_prefix(calib, cam1: str, cam2: str):
|
||
|
|
keys = calib["keys"]
|
||
|
|
direct = f"pair_{cam1}_{cam2}"
|
||
|
|
inv = f"pair_{cam2}_{cam1}"
|
||
|
|
|
||
|
|
if f"{direct}_map1x" in keys:
|
||
|
|
return direct, False
|
||
|
|
|
||
|
|
if f"{inv}_map1x" in keys:
|
||
|
|
return inv, True
|
||
|
|
|
||
|
|
raise RuntimeError(f"Par {cam1}<->{cam2} não encontrado no .npz")
|
||
|
|
|
||
|
|
|
||
|
|
def rectify_pair_from_calib(img1_bgr, img2_bgr, calib, cam1: str, cam2: str):
|
||
|
|
"""
|
||
|
|
Retifica duas imagens usando o par salvo no .npz.
|
||
|
|
|
||
|
|
Retorna imagens na ordem solicitada: cam1_rect, cam2_rect.
|
||
|
|
Se o par salvo estiver invertido, troca map1/map2 automaticamente.
|
||
|
|
"""
|
||
|
|
data = calib["data"]
|
||
|
|
image_w, image_h = calib["image_size"]
|
||
|
|
|
||
|
|
if img1_bgr.shape[1] != image_w or img1_bgr.shape[0] != image_h:
|
||
|
|
img1_bgr = cv2.resize(img1_bgr, (image_w, image_h), interpolation=cv2.INTER_AREA)
|
||
|
|
if img2_bgr.shape[1] != image_w or img2_bgr.shape[0] != image_h:
|
||
|
|
img2_bgr = cv2.resize(img2_bgr, (image_w, image_h), interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
prefix, inverted = get_pair_prefix(calib, cam1, cam2)
|
||
|
|
|
||
|
|
if not inverted:
|
||
|
|
map1x = data[f"{prefix}_map1x"]
|
||
|
|
map1y = data[f"{prefix}_map1y"]
|
||
|
|
map2x = data[f"{prefix}_map2x"]
|
||
|
|
map2y = data[f"{prefix}_map2y"]
|
||
|
|
else:
|
||
|
|
map1x = data[f"{prefix}_map2x"]
|
||
|
|
map1y = data[f"{prefix}_map2y"]
|
||
|
|
map2x = data[f"{prefix}_map1x"]
|
||
|
|
map2y = data[f"{prefix}_map1y"]
|
||
|
|
|
||
|
|
rect1 = cv2.remap(
|
||
|
|
img1_bgr,
|
||
|
|
map1x,
|
||
|
|
map1y,
|
||
|
|
interpolation=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
rect2 = cv2.remap(
|
||
|
|
img2_bgr,
|
||
|
|
map2x,
|
||
|
|
map2y,
|
||
|
|
interpolation=cv2.INTER_LINEAR,
|
||
|
|
borderMode=cv2.BORDER_CONSTANT,
|
||
|
|
borderValue=0,
|
||
|
|
)
|
||
|
|
|
||
|
|
return rect1, rect2
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Disparity RE/NIR from multicam npz
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def make_sgbm(args):
|
||
|
|
num_disp = max(16, int(round(args.num_disp / 16)) * 16)
|
||
|
|
|
||
|
|
block_size = max(3, int(args.block_size))
|
||
|
|
if block_size % 2 == 0:
|
||
|
|
block_size += 1
|
||
|
|
|
||
|
|
matcher = cv2.StereoSGBM_create(
|
||
|
|
minDisparity=args.min_disp,
|
||
|
|
numDisparities=num_disp,
|
||
|
|
blockSize=block_size,
|
||
|
|
P1=8 * block_size * block_size,
|
||
|
|
P2=32 * block_size * block_size,
|
||
|
|
disp12MaxDiff=1,
|
||
|
|
uniquenessRatio=args.uniqueness,
|
||
|
|
speckleWindowSize=args.speckle_window,
|
||
|
|
speckleRange=args.speckle_range,
|
||
|
|
preFilterCap=63,
|
||
|
|
mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY,
|
||
|
|
)
|
||
|
|
|
||
|
|
return matcher, num_disp, block_size
|
||
|
|
|
||
|
|
|
||
|
|
def compute_disparity(left_rect_bgr, right_rect_bgr, args):
|
||
|
|
left_gray = to_gray_u8(left_rect_bgr)
|
||
|
|
right_gray = to_gray_u8(right_rect_bgr)
|
||
|
|
|
||
|
|
matcher, num_disp, block_size = make_sgbm(args)
|
||
|
|
disp_raw = matcher.compute(left_gray, right_gray).astype(np.float32) / 16.0
|
||
|
|
valid = disp_raw > args.min_valid_disp
|
||
|
|
|
||
|
|
disp_vis = disp_raw.copy()
|
||
|
|
disp_vis[~valid] = 0.0
|
||
|
|
|
||
|
|
if np.count_nonzero(valid) > 20:
|
||
|
|
vals = disp_vis[valid]
|
||
|
|
p2 = np.percentile(vals, 2)
|
||
|
|
p98 = np.percentile(vals, 98)
|
||
|
|
disp_norm = (disp_vis - p2) / (p98 - p2 + 1e-6)
|
||
|
|
disp_norm = np.clip(disp_norm, 0.0, 1.0)
|
||
|
|
else:
|
||
|
|
disp_norm = np.zeros_like(disp_vis, dtype=np.float32)
|
||
|
|
|
||
|
|
disp_color = cv2.applyColorMap((disp_norm * 255).astype(np.uint8), cv2.COLORMAP_TURBO)
|
||
|
|
|
||
|
|
valid_mask = np.zeros_like(disp_color)
|
||
|
|
valid_mask[valid] = (255, 255, 255)
|
||
|
|
|
||
|
|
stats = {
|
||
|
|
"num_disp": num_disp,
|
||
|
|
"block_size": block_size,
|
||
|
|
"valid_pct": float(np.mean(valid) * 100.0),
|
||
|
|
"disp_p05": float(np.percentile(disp_vis[valid], 5)) if np.count_nonzero(valid) > 20 else 0.0,
|
||
|
|
"disp_p50": float(np.percentile(disp_vis[valid], 50)) if np.count_nonzero(valid) > 20 else 0.0,
|
||
|
|
"disp_p95": float(np.percentile(disp_vis[valid], 95)) if np.count_nonzero(valid) > 20 else 0.0,
|
||
|
|
}
|
||
|
|
|
||
|
|
return disp_raw, disp_color, valid_mask, stats
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Visualization
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def draw_epipolar_lines(img_bgr, step=40):
|
||
|
|
out = img_bgr.copy()
|
||
|
|
h, w = out.shape[:2]
|
||
|
|
|
||
|
|
for y in range(0, h, step):
|
||
|
|
color = (0, 255, 255) if (y // step) % 2 == 0 else (255, 255, 0)
|
||
|
|
cv2.line(out, (0, y), (w, y), color, 1, cv2.LINE_AA)
|
||
|
|
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def resize_to_height(img, target_h):
|
||
|
|
h, w = img.shape[:2]
|
||
|
|
if h == target_h:
|
||
|
|
return img
|
||
|
|
|
||
|
|
scale = target_h / h
|
||
|
|
new_w = max(1, int(w * scale))
|
||
|
|
return cv2.resize(img, (new_w, target_h), interpolation=cv2.INTER_AREA)
|
||
|
|
|
||
|
|
|
||
|
|
def draw_header(canvas, lines):
|
||
|
|
header_h = 24 + 24 * len(lines)
|
||
|
|
cv2.rectangle(canvas, (0, 0), (canvas.shape[1], header_h), (0, 0, 0), -1)
|
||
|
|
|
||
|
|
y = 24
|
||
|
|
for line in lines:
|
||
|
|
cv2.putText(
|
||
|
|
canvas,
|
||
|
|
line,
|
||
|
|
(12, y),
|
||
|
|
cv2.FONT_HERSHEY_SIMPLEX,
|
||
|
|
0.55,
|
||
|
|
(255, 255, 255),
|
||
|
|
1,
|
||
|
|
cv2.LINE_AA,
|
||
|
|
)
|
||
|
|
y += 24
|
||
|
|
|
||
|
|
return canvas
|
||
|
|
|
||
|
|
|
||
|
|
def put_label(img, text, color=(255, 255, 255)):
|
||
|
|
out = img.copy()
|
||
|
|
cv2.rectangle(out, (0, 0), (out.shape[1], 34), (0, 0, 0), -1)
|
||
|
|
cv2.putText(out, text, (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, color, 1, cv2.LINE_AA)
|
||
|
|
return out
|
||
|
|
|
||
|
|
|
||
|
|
def compose_three(left_bgr, center_bgr, right_bgr, title_lines, args):
|
||
|
|
if args.lines:
|
||
|
|
left_bgr = draw_epipolar_lines(left_bgr, step=args.line_step)
|
||
|
|
center_bgr = draw_epipolar_lines(center_bgr, step=args.line_step)
|
||
|
|
right_bgr = draw_epipolar_lines(right_bgr, step=args.line_step)
|
||
|
|
|
||
|
|
left = resize_to_height(left_bgr, args.view_h)
|
||
|
|
center = resize_to_height(center_bgr, args.view_h)
|
||
|
|
right = resize_to_height(right_bgr, args.view_h)
|
||
|
|
|
||
|
|
h = min(left.shape[0], center.shape[0], right.shape[0])
|
||
|
|
left = left[:h]
|
||
|
|
center = center[:h]
|
||
|
|
right = right[:h]
|
||
|
|
|
||
|
|
canvas = np.hstack([left, center, right])
|
||
|
|
return draw_header(canvas, title_lines)
|
||
|
|
|
||
|
|
|
||
|
|
def compose_four(a_bgr, b_bgr, c_bgr, d_bgr, title_lines, args):
|
||
|
|
if args.lines:
|
||
|
|
a_bgr = draw_epipolar_lines(a_bgr, step=args.line_step)
|
||
|
|
b_bgr = draw_epipolar_lines(b_bgr, step=args.line_step)
|
||
|
|
c_bgr = draw_epipolar_lines(c_bgr, step=args.line_step)
|
||
|
|
d_bgr = draw_epipolar_lines(d_bgr, step=args.line_step)
|
||
|
|
|
||
|
|
imgs = [resize_to_height(x, args.view_h) for x in [a_bgr, b_bgr, c_bgr, d_bgr]]
|
||
|
|
h = min(x.shape[0] for x in imgs)
|
||
|
|
imgs = [x[:h] for x in imgs]
|
||
|
|
canvas = np.hstack(imgs)
|
||
|
|
return draw_header(canvas, title_lines)
|
||
|
|
|
||
|
|
|
||
|
|
def absdiff_bgr(a, b):
|
||
|
|
ag = to_gray_u8(a)
|
||
|
|
bg = to_gray_u8(b)
|
||
|
|
diff = cv2.absdiff(ag, bg)
|
||
|
|
return cv2.cvtColor(diff, cv2.COLOR_GRAY2BGR)
|
||
|
|
|
||
|
|
|
||
|
|
def make_overlay(base_bgr, layer_bgr, alpha=0.45):
|
||
|
|
layer = cv2.resize(layer_bgr, (base_bgr.shape[1], base_bgr.shape[0]), interpolation=cv2.INTER_AREA)
|
||
|
|
return cv2.addWeighted(base_bgr, 1.0 - alpha, layer, alpha, 0)
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Frame loading and modes
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def load_triplet_images(item, calib, args):
|
||
|
|
rgb_cam = calib["rgb_cam"]
|
||
|
|
nir_cam = calib["nir_cam"]
|
||
|
|
re_cam = calib["re_cam"]
|
||
|
|
|
||
|
|
rgb = read_raw10_rgb_view_bgr(
|
||
|
|
item[rgb_cam],
|
||
|
|
args.width,
|
||
|
|
args.height,
|
||
|
|
args.rgb_bayer,
|
||
|
|
args.rgb_view,
|
||
|
|
use_clahe=not args.no_clahe,
|
||
|
|
)
|
||
|
|
nir = read_raw10_mono_bgr(item[nir_cam], args.width, args.height, use_clahe=not args.no_clahe)
|
||
|
|
re = read_raw10_mono_bgr(item[re_cam], args.width, args.height, use_clahe=not args.no_clahe)
|
||
|
|
|
||
|
|
return {
|
||
|
|
rgb_cam: rgb,
|
||
|
|
nir_cam: nir,
|
||
|
|
re_cam: re,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def build_views(images, calib, args):
|
||
|
|
rgb_cam = calib["rgb_cam"]
|
||
|
|
nir_cam = calib["nir_cam"]
|
||
|
|
re_cam = calib["re_cam"]
|
||
|
|
|
||
|
|
rgb = images[rgb_cam]
|
||
|
|
nir = images[nir_cam]
|
||
|
|
re = images[re_cam]
|
||
|
|
|
||
|
|
# RGB <-> NIR
|
||
|
|
rgb_ab, nir_ab = rectify_pair_from_calib(rgb, nir, calib, rgb_cam, nir_cam)
|
||
|
|
|
||
|
|
# RGB <-> RE
|
||
|
|
rgb_ac, re_ac = rectify_pair_from_calib(rgb, re, calib, rgb_cam, re_cam)
|
||
|
|
|
||
|
|
# RE <-> NIR
|
||
|
|
re_cb, nir_cb = rectify_pair_from_calib(re, nir, calib, re_cam, nir_cam)
|
||
|
|
|
||
|
|
_, disp_color, valid_mask, disp_stats = compute_disparity(re_cb, nir_cb, args)
|
||
|
|
|
||
|
|
return {
|
||
|
|
"rgb_native": rgb,
|
||
|
|
"nir_native": nir,
|
||
|
|
"re_native": re,
|
||
|
|
"rgb_ab": rgb_ab,
|
||
|
|
"nir_ab": nir_ab,
|
||
|
|
"rgb_ac": rgb_ac,
|
||
|
|
"re_ac": re_ac,
|
||
|
|
"re_cb": re_cb,
|
||
|
|
"nir_cb": nir_cb,
|
||
|
|
"disp_color": disp_color,
|
||
|
|
"valid_mask": valid_mask,
|
||
|
|
"disp_stats": disp_stats,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def compose_mode(views, item, idx, total, mode, calib, args):
|
||
|
|
rgb_cam = calib["rgb_cam"]
|
||
|
|
nir_cam = calib["nir_cam"]
|
||
|
|
re_cam = calib["re_cam"]
|
||
|
|
|
||
|
|
rgb_name = item[rgb_cam].name
|
||
|
|
|
||
|
|
if mode == "triple_native":
|
||
|
|
left = put_label(views["re_native"], "RE native")
|
||
|
|
center = put_label(views["rgb_native"], "RGB native REF")
|
||
|
|
right = put_label(views["nir_native"], "NIR native")
|
||
|
|
lines = [
|
||
|
|
f"{idx + 1}/{total} | {rgb_name}",
|
||
|
|
"mode=triple_native | sem retificação, RGB no centro",
|
||
|
|
"N/SPACE prox | A ant | M modo | L linhas | S salvar | Q sair",
|
||
|
|
]
|
||
|
|
return compose_three(left, center, right, lines, args)
|
||
|
|
|
||
|
|
if mode == "rgb_re":
|
||
|
|
left = put_label(views["re_ac"], "RE rectificado no par RGB-RE")
|
||
|
|
center = put_label(views["rgb_ac"], "RGB rectificado no par RGB-RE")
|
||
|
|
right = absdiff_bgr(views["rgb_ac"], views["re_ac"])
|
||
|
|
right = put_label(right, "diff RGB-RE")
|
||
|
|
lines = [
|
||
|
|
f"{idx + 1}/{total} | {rgb_name}",
|
||
|
|
"mode=rgb_re | valida alinhamento epipolar RGB<->RE",
|
||
|
|
"N/SPACE prox | A ant | M modo | L linhas | S salvar | Q sair",
|
||
|
|
]
|
||
|
|
return compose_three(left, center, right, lines, args)
|
||
|
|
|
||
|
|
if mode == "rgb_nir":
|
||
|
|
left = absdiff_bgr(views["rgb_ab"], views["nir_ab"])
|
||
|
|
left = put_label(left, "diff RGB-NIR")
|
||
|
|
center = put_label(views["rgb_ab"], "RGB rectificado no par RGB-NIR")
|
||
|
|
right = put_label(views["nir_ab"], "NIR rectificado no par RGB-NIR")
|
||
|
|
lines = [
|
||
|
|
f"{idx + 1}/{total} | {rgb_name}",
|
||
|
|
"mode=rgb_nir | valida alinhamento epipolar RGB<->NIR",
|
||
|
|
"N/SPACE prox | A ant | M modo | L linhas | S salvar | Q sair",
|
||
|
|
]
|
||
|
|
return compose_three(left, center, right, lines, args)
|
||
|
|
|
||
|
|
if mode == "re_nir":
|
||
|
|
left = put_label(views["re_cb"], "RE rectificado no par RE-NIR")
|
||
|
|
center = put_label(views["disp_color"], "disparity RE-NIR")
|
||
|
|
right = put_label(views["nir_cb"], "NIR rectificado no par RE-NIR")
|
||
|
|
st = views["disp_stats"]
|
||
|
|
lines = [
|
||
|
|
f"{idx + 1}/{total} | {rgb_name}",
|
||
|
|
f"mode=re_nir | valid={st['valid_pct']:.1f}% | disp p05={st['disp_p05']:.2f} p50={st['disp_p50']:.2f} p95={st['disp_p95']:.2f}",
|
||
|
|
f"numDisp={st['num_disp']} | block={st['block_size']} | linhas={args.lines}",
|
||
|
|
"N/SPACE prox | A ant | M modo | L linhas | [ ] numDisp | - + block | S salvar | Q sair",
|
||
|
|
]
|
||
|
|
return compose_three(left, center, right, lines, args)
|
||
|
|
|
||
|
|
if mode == "quad_pairs":
|
||
|
|
a = put_label(views["re_ac"], "RE em RGB-RE")
|
||
|
|
b = put_label(views["rgb_ac"], "RGB em RGB-RE")
|
||
|
|
c = put_label(views["rgb_ab"], "RGB em RGB-NIR")
|
||
|
|
d = put_label(views["nir_ab"], "NIR em RGB-NIR")
|
||
|
|
lines = [
|
||
|
|
f"{idx + 1}/{total} | {rgb_name}",
|
||
|
|
"mode=quad_pairs | mostra os dois mundos retificados que usam RGB",
|
||
|
|
"N/SPACE prox | A ant | M modo | L linhas | S salvar | Q sair",
|
||
|
|
]
|
||
|
|
return compose_four(a, b, c, d, lines, args)
|
||
|
|
|
||
|
|
raise RuntimeError(f"Modo desconhecido: {mode}")
|
||
|
|
|
||
|
|
|
||
|
|
# ============================================================
|
||
|
|
# Main
|
||
|
|
# ============================================================
|
||
|
|
|
||
|
|
def main():
|
||
|
|
parser = argparse.ArgumentParser()
|
||
|
|
|
||
|
|
parser.add_argument("--root_dir", required=True)
|
||
|
|
parser.add_argument("--calib_path", required=True)
|
||
|
|
|
||
|
|
parser.add_argument("--width", type=int, default=1280)
|
||
|
|
parser.add_argument("--height", type=int, default=800)
|
||
|
|
parser.add_argument("--rgb_bayer", default="BGGR", help="Use o mesmo padrão que funcionou na calibração. Ex: BGGR ou RGGB")
|
||
|
|
parser.add_argument(
|
||
|
|
"--rgb_view",
|
||
|
|
default="gray",
|
||
|
|
choices=["color", "gray", "raw_bayer_gray"],
|
||
|
|
help="Como mostrar/processar CAM_A no viewer. gray replica melhor a calibração.",
|
||
|
|
)
|
||
|
|
|
||
|
|
parser.add_argument("--view_h", type=int, default=420)
|
||
|
|
parser.add_argument("--no_clahe", action="store_true")
|
||
|
|
parser.add_argument("--lines", action="store_true")
|
||
|
|
parser.add_argument("--line_step", type=int, default=40)
|
||
|
|
|
||
|
|
parser.add_argument("--num_disp", type=int, default=128)
|
||
|
|
parser.add_argument("--block_size", type=int, default=7)
|
||
|
|
parser.add_argument("--min_disp", type=int, default=0)
|
||
|
|
parser.add_argument("--min_valid_disp", type=float, default=1.0)
|
||
|
|
parser.add_argument("--uniqueness", type=int, default=8)
|
||
|
|
parser.add_argument("--speckle_window", type=int, default=80)
|
||
|
|
parser.add_argument("--speckle_range", type=int, default=2)
|
||
|
|
|
||
|
|
parser.add_argument("--save_dir", default="multicam_rectified_viewer_saves")
|
||
|
|
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
root_dir = Path(args.root_dir)
|
||
|
|
calib_path = Path(args.calib_path)
|
||
|
|
save_dir = Path(args.save_dir)
|
||
|
|
save_dir.mkdir(parents=True, exist_ok=True)
|
||
|
|
|
||
|
|
calib = load_multicam_calib(calib_path)
|
||
|
|
|
||
|
|
rgb_cam = calib["rgb_cam"]
|
||
|
|
nir_cam = calib["nir_cam"]
|
||
|
|
re_cam = calib["re_cam"]
|
||
|
|
cams = [rgb_cam, nir_cam, re_cam]
|
||
|
|
|
||
|
|
print(f"[INFO] calib_path: {calib_path}")
|
||
|
|
print(f"[INFO] image_size: {calib['image_size']}")
|
||
|
|
print(f"[INFO] rgb_cam={rgb_cam} nir_cam={nir_cam} re_cam={re_cam} ref_cam={calib['ref_cam']}")
|
||
|
|
|
||
|
|
for k in calib["keys"]:
|
||
|
|
if k.startswith("pair_") and k.endswith("_rms"):
|
||
|
|
print(f"[INFO] {k}: {float(calib['data'][k]):.6f}")
|
||
|
|
if k.startswith("pair_") and k.endswith("_T"):
|
||
|
|
print(f"[INFO] {k}: {np.array(calib['data'][k]).ravel()}")
|
||
|
|
|
||
|
|
triplets, by_cam = find_triplets(root_dir, cams)
|
||
|
|
|
||
|
|
for cam in cams:
|
||
|
|
print(f"[INFO] arquivos {cam}: {len(by_cam[cam])}")
|
||
|
|
print(f"[INFO] triplets: {len(triplets)}")
|
||
|
|
|
||
|
|
if not triplets:
|
||
|
|
raise RuntimeError("Nenhum triplet CAM_A/CAM_B/CAM_C encontrado.")
|
||
|
|
|
||
|
|
modes = ["triple_native", "rgb_re", "rgb_nir", "re_nir", "quad_pairs"]
|
||
|
|
mode_idx = 0
|
||
|
|
|
||
|
|
idx = 0
|
||
|
|
cached_key = None
|
||
|
|
cached_views = None
|
||
|
|
|
||
|
|
cv2.namedWindow("Multicam RAW10 Rectified Viewer", cv2.WINDOW_NORMAL)
|
||
|
|
|
||
|
|
while True:
|
||
|
|
item = triplets[idx]
|
||
|
|
mode = modes[mode_idx]
|
||
|
|
|
||
|
|
key_cache = (
|
||
|
|
tuple(str(item[cam]) for cam in cams),
|
||
|
|
args.width,
|
||
|
|
args.height,
|
||
|
|
args.rgb_bayer,
|
||
|
|
args.rgb_view,
|
||
|
|
args.no_clahe,
|
||
|
|
str(calib_path),
|
||
|
|
args.num_disp,
|
||
|
|
args.block_size,
|
||
|
|
args.min_disp,
|
||
|
|
args.min_valid_disp,
|
||
|
|
args.uniqueness,
|
||
|
|
args.speckle_window,
|
||
|
|
args.speckle_range,
|
||
|
|
)
|
||
|
|
|
||
|
|
if key_cache != cached_key:
|
||
|
|
print(f"[RUN] {idx + 1}/{len(triplets)} - {item[rgb_cam].name}")
|
||
|
|
|
||
|
|
try:
|
||
|
|
images = load_triplet_images(item, calib, args)
|
||
|
|
cached_views = build_views(images, calib, args)
|
||
|
|
cached_key = key_cache
|
||
|
|
except Exception as e:
|
||
|
|
print("[ERRO] Falha processando triplet:")
|
||
|
|
for cam in cams:
|
||
|
|
print(f" {cam}={item[cam]}")
|
||
|
|
print(f" erro={e}")
|
||
|
|
idx = min(idx + 1, len(triplets) - 1)
|
||
|
|
cached_key = None
|
||
|
|
cached_views = None
|
||
|
|
continue
|
||
|
|
|
||
|
|
view = compose_mode(
|
||
|
|
views=cached_views,
|
||
|
|
item=item,
|
||
|
|
idx=idx,
|
||
|
|
total=len(triplets),
|
||
|
|
mode=mode,
|
||
|
|
calib=calib,
|
||
|
|
args=args,
|
||
|
|
)
|
||
|
|
|
||
|
|
cv2.imshow("Multicam RAW10 Rectified Viewer", view)
|
||
|
|
key = cv2.waitKey(0) & 0xFF
|
||
|
|
|
||
|
|
if key in [27, ord("q"), ord("Q")]:
|
||
|
|
break
|
||
|
|
|
||
|
|
elif key in [ord("n"), ord("N"), 32]:
|
||
|
|
idx = min(idx + 1, len(triplets) - 1)
|
||
|
|
cached_key = None
|
||
|
|
|
||
|
|
elif key in [ord("a"), ord("A")]:
|
||
|
|
idx = max(idx - 1, 0)
|
||
|
|
cached_key = None
|
||
|
|
|
||
|
|
elif key in [ord("m"), ord("M")]:
|
||
|
|
mode_idx = (mode_idx + 1) % len(modes)
|
||
|
|
print(f"[PARAM] mode={modes[mode_idx]}")
|
||
|
|
|
||
|
|
elif key in [ord("l"), ord("L")]:
|
||
|
|
args.lines = not args.lines
|
||
|
|
print(f"[PARAM] lines={args.lines}")
|
||
|
|
|
||
|
|
elif key == ord("["):
|
||
|
|
args.num_disp = max(16, args.num_disp - 16)
|
||
|
|
cached_key = None
|
||
|
|
print(f"[PARAM] num_disp={args.num_disp}")
|
||
|
|
|
||
|
|
elif key == ord("]"):
|
||
|
|
args.num_disp = min(512, args.num_disp + 16)
|
||
|
|
cached_key = None
|
||
|
|
print(f"[PARAM] num_disp={args.num_disp}")
|
||
|
|
|
||
|
|
elif key in [ord("-"), ord("_")]:
|
||
|
|
args.block_size = max(3, args.block_size - 2)
|
||
|
|
if args.block_size % 2 == 0:
|
||
|
|
args.block_size -= 1
|
||
|
|
cached_key = None
|
||
|
|
print(f"[PARAM] block_size={args.block_size}")
|
||
|
|
|
||
|
|
elif key in [ord("+"), ord("=")]:
|
||
|
|
args.block_size = min(31, args.block_size + 2)
|
||
|
|
if args.block_size % 2 == 0:
|
||
|
|
args.block_size += 1
|
||
|
|
cached_key = None
|
||
|
|
print(f"[PARAM] block_size={args.block_size}")
|
||
|
|
|
||
|
|
elif key in [ord("s"), ord("S")]:
|
||
|
|
out_path = save_dir / f"multicam_{idx:04d}_{mode}.png"
|
||
|
|
cv2.imwrite(str(out_path), view)
|
||
|
|
print(f"[SAVE] {out_path}")
|
||
|
|
|
||
|
|
cv2.destroyAllWindows()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|