Initial commit: Speckle-Scanner 3D pipeline with setup README
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,424 @@
|
||||
"""Step 2: mono and stereo calibration from per-image JSON feature files."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from calibrationclasses.feature_json import FeatureRecord, load_folder_features
|
||||
from calibrationclasses.pairing import StereoPair, build_stereo_pairs
|
||||
from calibrationclasses.session import (
|
||||
CameraFolder,
|
||||
discover_camera_folder,
|
||||
resolve_session_root,
|
||||
STEREO_PARTNERS,
|
||||
)
|
||||
|
||||
|
||||
def create_3d_board_points(board_size: Tuple[int, int], square_size: float) -> np.ndarray:
|
||||
pts = np.zeros((np.prod(board_size), 3), np.float32)
|
||||
pts[:, :2] = np.indices(board_size).T.reshape(-1, 2)
|
||||
pts *= square_size
|
||||
return pts
|
||||
|
||||
|
||||
def _image_size_from_records(records: List[FeatureRecord]) -> Tuple[int, int]:
|
||||
for record in records:
|
||||
img = cv2.imread(str(record.image_path))
|
||||
if img is not None:
|
||||
return img.shape[1], img.shape[0]
|
||||
raise RuntimeError("Could not determine image size from feature JSONs")
|
||||
|
||||
|
||||
def calibrate_camera_intrinsics(
|
||||
records: List[FeatureRecord],
|
||||
board_size: Tuple[int, int],
|
||||
square_size: float,
|
||||
) -> Dict:
|
||||
chess_records = [r for r in records if r.is_chessboard]
|
||||
if len(chess_records) < 3:
|
||||
raise RuntimeError(
|
||||
f"Need at least 3 chessboard detections for mono calibration, got {len(chess_records)}"
|
||||
)
|
||||
|
||||
image_size = _image_size_from_records(chess_records)
|
||||
objp = create_3d_board_points(board_size, square_size)
|
||||
obj_points = [objp for _ in chess_records]
|
||||
img_points = [r.corners for r in chess_records]
|
||||
|
||||
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
|
||||
obj_points, img_points, image_size, None, None, flags=0
|
||||
)
|
||||
|
||||
rmtx = []
|
||||
tmtx = []
|
||||
for k, r in enumerate(rvecs):
|
||||
rmtx.append(cv2.Rodrigues(r)[0])
|
||||
tmtx.append(np.vstack((np.hstack((rmtx[k], tvecs[k])), np.array([0, 0, 0, 1]))))
|
||||
|
||||
newmtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, image_size, 1, image_size)
|
||||
if np.sum(roi) == 0:
|
||||
roi = (0, 0, image_size[0] - 1, image_size[1] - 1)
|
||||
|
||||
return {
|
||||
"Intrinsic": mtx,
|
||||
"Distortion": dist,
|
||||
"DistortionROI": roi,
|
||||
"DistortionIntrinsic": newmtx,
|
||||
"RotVektor": rvecs,
|
||||
"RotMatrix": rmtx,
|
||||
"Extrinsics": tmtx,
|
||||
"TransVektor": tvecs,
|
||||
"MeanError": float(ret),
|
||||
"image_size": image_size,
|
||||
"num_views": len(chess_records),
|
||||
}
|
||||
|
||||
|
||||
def save_mono_intrinsics(
|
||||
params_dir: Path,
|
||||
camera_name: str,
|
||||
intrinsics: Dict,
|
||||
*,
|
||||
troubleshooting: bool = False,
|
||||
) -> None:
|
||||
params_dir.mkdir(parents=True, exist_ok=True)
|
||||
tag = camera_name.replace("/", "-")
|
||||
npz_path = params_dir / f"{tag}_intrinsics.npz"
|
||||
np.savez(
|
||||
npz_path,
|
||||
Intrinsic=intrinsics["Intrinsic"],
|
||||
Distortion=intrinsics["Distortion"],
|
||||
DistortionIntrinsic=intrinsics["DistortionIntrinsic"],
|
||||
DistortionROI=intrinsics["DistortionROI"],
|
||||
MeanError=intrinsics["MeanError"],
|
||||
image_size=np.array(intrinsics["image_size"]),
|
||||
num_views=intrinsics["num_views"],
|
||||
)
|
||||
yaml_path = params_dir / f"{tag}_intrinsics.yaml"
|
||||
fs = cv2.FileStorage(str(yaml_path), cv2.FILE_STORAGE_WRITE)
|
||||
fs.write("Intrinsic", intrinsics["Intrinsic"])
|
||||
fs.write("Distortion", intrinsics["Distortion"])
|
||||
fs.write("DistortionIntrinsic", intrinsics["DistortionIntrinsic"])
|
||||
fs.release()
|
||||
if troubleshooting:
|
||||
print(f"[INFO] Saved mono intrinsics → {npz_path} and {yaml_path}")
|
||||
|
||||
|
||||
def run_mono_calibration(
|
||||
input_path: str | Path,
|
||||
board_sizes: Dict[str, Tuple[int, int]],
|
||||
square_sizes: Dict[str, float],
|
||||
cameras: Optional[List[str]] = None,
|
||||
troubleshooting: bool = False,
|
||||
) -> Dict[str, Dict]:
|
||||
session_root = resolve_session_root(input_path)
|
||||
params_dir = Path(input_path) / "params"
|
||||
results = {}
|
||||
|
||||
for logical_name, board_size in board_sizes.items():
|
||||
if cameras and logical_name not in cameras:
|
||||
continue
|
||||
cam = discover_camera_folder(session_root, logical_name)
|
||||
if cam is None:
|
||||
continue
|
||||
records = load_folder_features(cam.path)
|
||||
square_size = square_sizes[logical_name]
|
||||
try:
|
||||
intrinsics = calibrate_camera_intrinsics(records, board_size, square_size)
|
||||
save_mono_intrinsics(
|
||||
params_dir, logical_name, intrinsics, troubleshooting=troubleshooting
|
||||
)
|
||||
results[logical_name] = intrinsics
|
||||
print(
|
||||
f"[mono:{logical_name}] views={intrinsics['num_views']} "
|
||||
f"reproj_err={intrinsics['MeanError']:.4f}"
|
||||
)
|
||||
except RuntimeError as exc:
|
||||
if troubleshooting:
|
||||
print(f"[SKIP mono:{logical_name}] {exc}")
|
||||
else:
|
||||
print(f"[mono:{logical_name}] skipped")
|
||||
|
||||
return results
|
||||
|
||||
|
||||
def calibrate_stereo_pair(
|
||||
pairs: List[StereoPair],
|
||||
left_intrinsics: Dict,
|
||||
right_intrinsics: Dict,
|
||||
board_size: Tuple[int, int],
|
||||
square_size: float,
|
||||
image_size: Tuple[int, int],
|
||||
) -> Dict:
|
||||
if not pairs:
|
||||
raise RuntimeError("No stereo pairs available")
|
||||
|
||||
objp = create_3d_board_points(board_size, square_size)
|
||||
obj_points = [objp for _ in pairs]
|
||||
left_img_points = [p.left.corners for p in pairs]
|
||||
right_img_points = [p.right.corners for p in pairs]
|
||||
|
||||
flags = cv2.CALIB_FIX_INTRINSIC
|
||||
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 30, 0.001)
|
||||
|
||||
ret_stereo, _, _, _, _, rot, trans, essential, fundamental = cv2.stereoCalibrate(
|
||||
obj_points,
|
||||
left_img_points,
|
||||
right_img_points,
|
||||
left_intrinsics["Intrinsic"],
|
||||
left_intrinsics["Distortion"],
|
||||
right_intrinsics["Intrinsic"],
|
||||
right_intrinsics["Distortion"],
|
||||
image_size,
|
||||
criteria=criteria,
|
||||
flags=flags,
|
||||
)
|
||||
|
||||
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
|
||||
left_intrinsics["Intrinsic"],
|
||||
left_intrinsics["Distortion"],
|
||||
right_intrinsics["Intrinsic"],
|
||||
right_intrinsics["Distortion"],
|
||||
image_size,
|
||||
rot,
|
||||
trans,
|
||||
flags=0,
|
||||
alpha=1,
|
||||
)
|
||||
|
||||
T = np.vstack((np.hstack((rot, trans)), np.array([0, 0, 0, 1])))
|
||||
Q_clean = np.array(Q, dtype=np.float64)
|
||||
|
||||
parameters = {
|
||||
"Translation": trans,
|
||||
"Rotation": rot,
|
||||
"Transformation": T,
|
||||
"Essential": essential,
|
||||
"Fundamental": fundamental,
|
||||
"MeanError": float(ret_stereo),
|
||||
"SquareSize": square_size,
|
||||
"BoardSize": board_size,
|
||||
"Objpoints": objp,
|
||||
"Q": Q_clean,
|
||||
"num_pairs": len(pairs),
|
||||
"L_Intrinsic": left_intrinsics["Intrinsic"],
|
||||
"L_Distortion": left_intrinsics["Distortion"],
|
||||
"L_DistortionIntrinsic": left_intrinsics["DistortionIntrinsic"],
|
||||
"R_Intrinsic": right_intrinsics["Intrinsic"],
|
||||
"R_Distortion": right_intrinsics["Distortion"],
|
||||
"R_DistortionIntrinsic": right_intrinsics["DistortionIntrinsic"],
|
||||
"L_Imgpoints": left_img_points,
|
||||
"R_Imgpoints": right_img_points,
|
||||
"R1": R1,
|
||||
"R2": R2,
|
||||
"P1": P1,
|
||||
"P2": P2,
|
||||
"image_size": image_size,
|
||||
}
|
||||
return parameters
|
||||
|
||||
|
||||
def save_stereo_calibration(
|
||||
input_path: str | Path,
|
||||
pair_tag: str,
|
||||
parameters: Dict,
|
||||
*,
|
||||
troubleshooting: bool = False,
|
||||
) -> None:
|
||||
params_dir = Path(input_path) / "params"
|
||||
params_dir.mkdir(parents=True, exist_ok=True)
|
||||
Q_clean = np.array(parameters["Q"], dtype=np.float64)
|
||||
|
||||
npz_path = params_dir / f"{pair_tag}_parameters.npz"
|
||||
save_kwargs = {k: v for k, v in parameters.items() if k not in ("R1", "R2", "P1", "P2")}
|
||||
np.savez(npz_path, **save_kwargs)
|
||||
if troubleshooting:
|
||||
print(f"[INFO] Saved NPZ → {npz_path}")
|
||||
|
||||
yaml_path = params_dir / f"{pair_tag}_stereo_cam_model.yaml"
|
||||
fs = cv2.FileStorage(str(yaml_path), cv2.FILE_STORAGE_WRITE)
|
||||
fs.write("L_DistortionIntrinsic", parameters["L_DistortionIntrinsic"])
|
||||
fs.write("L_Intrinsic", parameters["L_Intrinsic"])
|
||||
fs.write("L_Distortion", parameters["L_Distortion"])
|
||||
fs.write("R_DistortionIntrinsic", parameters["R_DistortionIntrinsic"])
|
||||
fs.write("R_Intrinsic", parameters["R_Intrinsic"])
|
||||
fs.write("R_Distortion", parameters["R_Distortion"])
|
||||
fs.write("Rotation", parameters["Transformation"][:3, :3])
|
||||
fs.write("Translation", parameters["Transformation"][:3, 3:])
|
||||
fs.write("Q", Q_clean)
|
||||
fs.release()
|
||||
if troubleshooting:
|
||||
print(f"[INFO] Saved YAML → {yaml_path}")
|
||||
|
||||
cvstore_path = params_dir / f"{pair_tag}_Q.cvstore"
|
||||
fs2 = cv2.FileStorage(str(cvstore_path), cv2.FILE_STORAGE_WRITE)
|
||||
fs2.write("Q", Q_clean)
|
||||
fs2.release()
|
||||
if troubleshooting:
|
||||
print(f"[INFO] Saved Q → {cvstore_path}")
|
||||
|
||||
|
||||
def save_pairing_report(
|
||||
input_path: str | Path,
|
||||
pair_tag: str,
|
||||
pairs: List[StereoPair],
|
||||
) -> Path:
|
||||
report_dir = Path(input_path) / "pairing_reports"
|
||||
report_dir.mkdir(parents=True, exist_ok=True)
|
||||
report_path = report_dir / f"{pair_tag}.txt"
|
||||
lines = [
|
||||
f"# stereo pairs for {pair_tag}",
|
||||
f"# total={len(pairs)}",
|
||||
"left_image\tright_image\tdelta_sec\tmethod",
|
||||
]
|
||||
for pair in pairs:
|
||||
lines.append(
|
||||
f"{pair.left.image_path.name}\t{pair.right.image_path.name}\t"
|
||||
f"{pair.delta_sec:.6f}\t{pair.method}"
|
||||
)
|
||||
report_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
print(f"[INFO] Pairing report → {report_path}")
|
||||
return report_path
|
||||
|
||||
|
||||
def save_rectified_pairs(
|
||||
input_path: str | Path,
|
||||
pair_tag: str,
|
||||
pairs: List[StereoPair],
|
||||
parameters: Dict,
|
||||
left_folder: str,
|
||||
right_folder: str,
|
||||
) -> None:
|
||||
image_size = parameters["image_size"]
|
||||
R1, R2, P1, P2 = parameters["R1"], parameters["R2"], parameters["P1"], parameters["P2"]
|
||||
map_left = cv2.initUndistortRectifyMap(
|
||||
parameters["L_Intrinsic"],
|
||||
parameters["L_Distortion"],
|
||||
R1,
|
||||
P1,
|
||||
image_size,
|
||||
cv2.CV_32FC1,
|
||||
)
|
||||
map_right = cv2.initUndistortRectifyMap(
|
||||
parameters["R_Intrinsic"],
|
||||
parameters["R_Distortion"],
|
||||
R2,
|
||||
P2,
|
||||
image_size,
|
||||
cv2.CV_32FC1,
|
||||
)
|
||||
|
||||
out_left = Path(input_path) / "rectified" / pair_tag / left_folder
|
||||
out_right = Path(input_path) / "rectified" / pair_tag / right_folder
|
||||
out_left.mkdir(parents=True, exist_ok=True)
|
||||
out_right.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
saved = 0
|
||||
for pair in pairs:
|
||||
left_img = cv2.imread(str(pair.left.image_path))
|
||||
right_img = cv2.imread(str(pair.right.image_path))
|
||||
if left_img is None or right_img is None:
|
||||
continue
|
||||
left_rect = cv2.remap(left_img, map_left[0], map_left[1], cv2.INTER_LINEAR)
|
||||
right_rect = cv2.remap(right_img, map_right[0], map_right[1], cv2.INTER_LINEAR)
|
||||
cv2.imwrite(str(out_left / pair.left.image_path.name), left_rect)
|
||||
cv2.imwrite(str(out_right / pair.right.image_path.name), right_rect)
|
||||
saved += 1
|
||||
print(f"[INFO] Rectified {saved}/{len(pairs)} pairs → {out_left.parent}")
|
||||
|
||||
|
||||
def run_stereo_calibration(
|
||||
input_path: str | Path,
|
||||
left_camera: str,
|
||||
mono_results: Dict[str, Dict],
|
||||
board_sizes: Dict[str, Tuple[int, int]],
|
||||
square_sizes: Dict[str, float],
|
||||
time_window_sec: float = 0.1,
|
||||
partners: Tuple[str, ...] = STEREO_PARTNERS,
|
||||
troubleshooting: bool = False,
|
||||
) -> None:
|
||||
session_root = resolve_session_root(input_path)
|
||||
left_cam = discover_camera_folder(session_root, left_camera)
|
||||
if left_cam is None:
|
||||
raise FileNotFoundError(f"Left camera folder {left_camera!r} not found")
|
||||
|
||||
if left_camera not in mono_results:
|
||||
raise RuntimeError(
|
||||
f"No mono intrinsics for {left_camera}. Run mono calibration first."
|
||||
)
|
||||
|
||||
left_records = load_folder_features(left_cam.path)
|
||||
left_board = board_sizes[left_camera]
|
||||
left_square = square_sizes[left_camera]
|
||||
image_size = mono_results[left_camera]["image_size"]
|
||||
|
||||
for partner in partners:
|
||||
right_cam = discover_camera_folder(session_root, partner)
|
||||
if right_cam is None:
|
||||
if troubleshooting:
|
||||
print(f"[SKIP stereo:{left_camera}-{partner}] folder not found")
|
||||
continue
|
||||
if partner not in mono_results:
|
||||
if troubleshooting:
|
||||
print(
|
||||
f"[SKIP stereo:{left_camera}-{partner}] "
|
||||
f"no mono intrinsics for {partner}"
|
||||
)
|
||||
continue
|
||||
|
||||
right_records = load_folder_features(right_cam.path)
|
||||
pairs = build_stereo_pairs(left_records, right_records, time_window_sec)
|
||||
pair_tag = f"{left_camera}-{partner}"
|
||||
|
||||
if not pairs:
|
||||
if troubleshooting:
|
||||
print(
|
||||
f"[SKIP stereo:{pair_tag}] no valid pairs "
|
||||
f"(time_window={time_window_sec}s)"
|
||||
)
|
||||
continue
|
||||
|
||||
time_n = sum(1 for p in pairs if p.method == "time_window")
|
||||
key_n = sum(1 for p in pairs if p.method == "pair_key")
|
||||
if troubleshooting:
|
||||
print(
|
||||
f"[stereo:{pair_tag}] {len(pairs)} pairs "
|
||||
f"(time_window={time_n}, pair_key={key_n})"
|
||||
)
|
||||
save_pairing_report(input_path, pair_tag, pairs)
|
||||
|
||||
try:
|
||||
params = calibrate_stereo_pair(
|
||||
pairs,
|
||||
mono_results[left_camera],
|
||||
mono_results[partner],
|
||||
left_board,
|
||||
left_square,
|
||||
image_size,
|
||||
)
|
||||
save_stereo_calibration(
|
||||
input_path, pair_tag, params, troubleshooting=troubleshooting
|
||||
)
|
||||
print(
|
||||
f"[stereo:{pair_tag}] pairs={params['num_pairs']} "
|
||||
f"reproj_err={params['MeanError']:.4f}"
|
||||
)
|
||||
if troubleshooting:
|
||||
save_rectified_pairs(
|
||||
input_path,
|
||||
pair_tag,
|
||||
pairs,
|
||||
params,
|
||||
left_cam.folder_name,
|
||||
right_cam.folder_name,
|
||||
)
|
||||
except RuntimeError as exc:
|
||||
if troubleshooting:
|
||||
print(f"[FAIL stereo:{pair_tag}] {exc}")
|
||||
else:
|
||||
print(f"[stereo:{pair_tag}] failed")
|
||||
@@ -0,0 +1,50 @@
|
||||
import numpy as np
|
||||
import matplotlib as mpl
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Patch
|
||||
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
|
||||
|
||||
class CameraPoseVisualizer:
|
||||
def __init__(self, xlim, ylim, zlim):
|
||||
self.fig = plt.figure(figsize=(18, 7))
|
||||
self.ax = self.fig.add_subplot(projection='3d')
|
||||
self.ax.set_aspect("auto")
|
||||
self.ax.set_xlim(xlim)
|
||||
self.ax.set_ylim(ylim)
|
||||
self.ax.set_zlim(zlim)
|
||||
self.ax.set_xlabel('x')
|
||||
self.ax.set_ylabel('y')
|
||||
self.ax.set_zlabel('z')
|
||||
print('initialize camera pose visualizer')
|
||||
|
||||
def extrinsic2pyramid(self, extrinsic, color='r', focal_len_scaled=5, aspect_ratio=0.3):
|
||||
vertex_std = np.array([[0, 0, 0, 1],
|
||||
[focal_len_scaled * aspect_ratio, -focal_len_scaled * aspect_ratio, focal_len_scaled, 1],
|
||||
[focal_len_scaled * aspect_ratio, focal_len_scaled * aspect_ratio, focal_len_scaled, 1],
|
||||
[-focal_len_scaled * aspect_ratio, focal_len_scaled * aspect_ratio, focal_len_scaled, 1],
|
||||
[-focal_len_scaled * aspect_ratio, -focal_len_scaled * aspect_ratio, focal_len_scaled, 1]])
|
||||
vertex_transformed = vertex_std @ extrinsic.T
|
||||
meshes = [[vertex_transformed[0, :-1], vertex_transformed[1][:-1], vertex_transformed[2, :-1]],
|
||||
[vertex_transformed[0, :-1], vertex_transformed[2, :-1], vertex_transformed[3, :-1]],
|
||||
[vertex_transformed[0, :-1], vertex_transformed[3, :-1], vertex_transformed[4, :-1]],
|
||||
[vertex_transformed[0, :-1], vertex_transformed[4, :-1], vertex_transformed[1, :-1]],
|
||||
[vertex_transformed[1, :-1], vertex_transformed[2, :-1], vertex_transformed[3, :-1], vertex_transformed[4, :-1]]]
|
||||
self.ax.add_collection3d(
|
||||
Poly3DCollection(meshes, facecolors=color, linewidths=0.3, edgecolors=color, alpha=0.35))
|
||||
|
||||
def customize_legend(self, list_label):
|
||||
list_handle = []
|
||||
for idx, label in enumerate(list_label):
|
||||
color = plt.cm.rainbow(idx / len(list_label))
|
||||
patch = Patch(color=color, label=label)
|
||||
list_handle.append(patch)
|
||||
plt.legend(loc='right', bbox_to_anchor=(1.8, 0.5), handles=list_handle)
|
||||
|
||||
def colorbar(self, max_frame_length):
|
||||
cmap = mpl.cm.rainbow
|
||||
norm = mpl.colors.Normalize(vmin=0, vmax=max_frame_length)
|
||||
self.fig.colorbar(mpl.cm.ScalarMappable(norm=norm, cmap=cmap), orientation='vertical', label='Frame Number')
|
||||
|
||||
def show(self):
|
||||
plt.title('Extrinsic Parameters')
|
||||
plt.show()
|
||||
@@ -0,0 +1,93 @@
|
||||
"""Shared CLI helpers for calibration scripts."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Tuple
|
||||
|
||||
import config
|
||||
|
||||
|
||||
def parse_chessboard_size(value: str) -> Tuple[int, int]:
|
||||
parts = value.split(",")
|
||||
if len(parts) != 2:
|
||||
raise argparse.ArgumentTypeError(
|
||||
"chessboard size must be width,height (e.g. 8,7)"
|
||||
)
|
||||
return tuple(map(int, parts))
|
||||
|
||||
|
||||
def add_session_args(parser: argparse.ArgumentParser) -> None:
|
||||
parser.add_argument("--project", required=True, help="Project name (e.g. Olsen_wings)")
|
||||
parser.add_argument("--date", required=True, help="Date string (e.g. 2026-05-12)")
|
||||
parser.add_argument(
|
||||
"--calib_name", default="calib1", help="Calibration folder name (default: calib1)"
|
||||
)
|
||||
|
||||
|
||||
def add_board_args(parser: argparse.ArgumentParser) -> None:
|
||||
parser.add_argument(
|
||||
"--chessboard_size",
|
||||
type=parse_chessboard_size,
|
||||
default="8,7",
|
||||
help="Default inner corner grid width,height",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--square_size",
|
||||
type=float,
|
||||
default=0.045,
|
||||
help="Default chessboard square size in metres",
|
||||
)
|
||||
parser.add_argument("--left_chessboard_size", type=parse_chessboard_size, default=None)
|
||||
parser.add_argument("--right_chessboard_size", type=parse_chessboard_size, default=None)
|
||||
parser.add_argument("--left_square_size", type=float, default=None)
|
||||
parser.add_argument("--right_square_size", type=float, default=None)
|
||||
parser.add_argument(
|
||||
"--preprocessing",
|
||||
type=str,
|
||||
default="None",
|
||||
help="Pre-detection chain: G=gray, C=CLAHE, T=threshold (e.g. C, GC)",
|
||||
)
|
||||
|
||||
|
||||
def add_troubleshooting_arg(parser: argparse.ArgumentParser) -> None:
|
||||
parser.add_argument(
|
||||
"--troubleshooting",
|
||||
action="store_true",
|
||||
help=(
|
||||
"Verbose logs and intermediate debug files (corners/, pairing_reports/, "
|
||||
"rectified/). Default: minimal logs; step 2 writes only params/"
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def resolve_input_path(args) -> Path:
|
||||
return config.CALIB_DATA_DIR / args.project / args.date / args.calib_name
|
||||
|
||||
|
||||
def build_board_config(args) -> Tuple[Dict[str, Tuple[int, int]], Dict[str, float]]:
|
||||
default_board = args.chessboard_size
|
||||
default_square = args.square_size
|
||||
left_board = args.left_chessboard_size or default_board
|
||||
right_board = args.right_chessboard_size or default_board
|
||||
left_square = args.left_square_size if args.left_square_size is not None else default_square
|
||||
right_square = (
|
||||
args.right_square_size if args.right_square_size is not None else default_square
|
||||
)
|
||||
|
||||
board_sizes = {
|
||||
"lc": left_board,
|
||||
"lc-ir": left_board,
|
||||
"rc": right_board,
|
||||
"rg": right_board,
|
||||
"ir": right_board,
|
||||
}
|
||||
square_sizes = {
|
||||
"lc": left_square,
|
||||
"lc-ir": left_square,
|
||||
"rc": right_square,
|
||||
"rg": right_square,
|
||||
"ir": right_square,
|
||||
}
|
||||
return board_sizes, square_sizes
|
||||
@@ -0,0 +1,280 @@
|
||||
"""Step 1: detect chessboard corners / ellipse centers and write per-image JSON."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from tqdm import tqdm
|
||||
|
||||
from calibrationclasses.feature_json import FeatureRecord, save_feature_json
|
||||
from calibrationclasses.preprocessing import Preprocessing
|
||||
from calibrationclasses.session import (
|
||||
CameraFolder,
|
||||
json_path_for_image,
|
||||
list_cameras_present,
|
||||
list_image_paths,
|
||||
resolve_session_root,
|
||||
)
|
||||
from calibrationclasses.timestamp import parse_pair_key, parse_timestamp_sec
|
||||
|
||||
|
||||
@dataclass
|
||||
class DetectionConfig:
|
||||
chessboard_size: Tuple[int, int] = (8, 7)
|
||||
square_size: float = 0.045
|
||||
preprocessing: str = "None"
|
||||
ir_mode: str = "auto" # auto | chessboard | ellipse
|
||||
troubleshooting: bool = False
|
||||
|
||||
|
||||
class FeatureDetector:
|
||||
def __init__(self, config: DetectionConfig, corners_root: Optional[Path] = None):
|
||||
self.config = config
|
||||
self._preprocessor = Preprocessing()
|
||||
self.corners_root = corners_root
|
||||
|
||||
def _preprocessing_enabled(self) -> bool:
|
||||
spec = (self.config.preprocessing or "").strip().lower()
|
||||
return bool(spec) and spec not in ("none", "off", "false", "0")
|
||||
|
||||
def _preprocess(self, image: np.ndarray) -> np.ndarray:
|
||||
if image is None or not self._preprocessing_enabled():
|
||||
return image
|
||||
spec = (
|
||||
(self.config.preprocessing or "")
|
||||
.strip()
|
||||
.lower()
|
||||
.replace("none", "")
|
||||
.replace(",", "")
|
||||
.replace(" ", "")
|
||||
)
|
||||
out = image
|
||||
pp = self._preprocessor
|
||||
for ch in spec:
|
||||
if ch == "g":
|
||||
g = pp.gray(out)
|
||||
out = cv2.cvtColor(g, cv2.COLOR_GRAY2BGR)
|
||||
elif ch == "c":
|
||||
c = pp.clahe(out)
|
||||
out = cv2.cvtColor(c, cv2.COLOR_GRAY2BGR)
|
||||
elif ch == "t":
|
||||
t = pp.threshold(out)
|
||||
out = cv2.cvtColor(t, cv2.COLOR_GRAY2BGR)
|
||||
return out
|
||||
|
||||
@staticmethod
|
||||
def _to_gray(image: np.ndarray) -> np.ndarray:
|
||||
if len(image.shape) == 2:
|
||||
return image
|
||||
return cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
def detect_chessboard(
|
||||
self, image: np.ndarray, board_size: Tuple[int, int]
|
||||
) -> Optional[np.ndarray]:
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
found, corners = cv2.findChessboardCorners(image, board_size, None)
|
||||
if not found:
|
||||
return None
|
||||
corners = cv2.cornerSubPix(
|
||||
self._to_gray(image), corners, (11, 11), (-1, -1), criteria
|
||||
)
|
||||
return corners
|
||||
|
||||
def detect_ellipse(self, image: np.ndarray):
|
||||
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
enhanced = clahe.apply(gray)
|
||||
blurred = cv2.GaussianBlur(enhanced, (5, 5), 0)
|
||||
_, binary = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
|
||||
if np.sum(binary == 255) / binary.size > 0.5:
|
||||
binary = cv2.bitwise_not(binary)
|
||||
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (9, 9))
|
||||
closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
|
||||
contours, _ = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
if not contours:
|
||||
return None
|
||||
valid = []
|
||||
for cnt in contours:
|
||||
area = cv2.contourArea(cnt)
|
||||
if area < 100:
|
||||
continue
|
||||
x, y, w, h = cv2.boundingRect(cnt)
|
||||
if 0.75 < (w / h) < 1.25:
|
||||
valid.append(cnt)
|
||||
if not valid:
|
||||
return None
|
||||
best = max(valid, key=cv2.contourArea)
|
||||
if len(best) < 5:
|
||||
return None
|
||||
ellipse = cv2.fitEllipse(best)
|
||||
(cx, cy), (major, minor), angle = ellipse
|
||||
return (cx, cy), {
|
||||
"center": [float(cx), float(cy)],
|
||||
"axes": [float(major), float(minor)],
|
||||
"angle": float(angle),
|
||||
}
|
||||
|
||||
def _save_corner_overlay(
|
||||
self,
|
||||
image: np.ndarray,
|
||||
record: FeatureRecord,
|
||||
board_size: Tuple[int, int],
|
||||
) -> None:
|
||||
if self.corners_root is None:
|
||||
return
|
||||
out_dir = self.corners_root / record.camera_folder
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
vis = image.copy()
|
||||
if record.feature_type == "chessboard" and record.corners is not None:
|
||||
vis = cv2.drawChessboardCorners(vis, board_size, record.corners, True)
|
||||
elif record.feature_type == "ellipse" and record.center is not None:
|
||||
cx, cy = record.center
|
||||
cv2.circle(vis, (int(cx), int(cy)), 12, (0, 255, 0), 2)
|
||||
out_path = out_dir / record.image_path.name
|
||||
cv2.imwrite(str(out_path), vis)
|
||||
|
||||
def process_image(
|
||||
self,
|
||||
image_path: Path,
|
||||
camera: CameraFolder,
|
||||
board_size: Optional[Tuple[int, int]] = None,
|
||||
square_size: Optional[float] = None,
|
||||
) -> FeatureRecord:
|
||||
board_size = board_size or self.config.chessboard_size
|
||||
square_size = square_size if square_size is not None else self.config.square_size
|
||||
json_path = json_path_for_image(image_path)
|
||||
|
||||
base = FeatureRecord(
|
||||
image_path=image_path,
|
||||
json_path=json_path,
|
||||
camera_folder=camera.folder_name,
|
||||
feature_type="unknown",
|
||||
success=False,
|
||||
preprocessing=self.config.preprocessing,
|
||||
timestamp_sec=parse_timestamp_sec(image_path.name),
|
||||
pair_key=parse_pair_key(image_path.name),
|
||||
)
|
||||
|
||||
image = cv2.imread(str(image_path))
|
||||
if image is None:
|
||||
base.error = "failed to load image"
|
||||
save_feature_json(base)
|
||||
return base
|
||||
|
||||
proc = self._preprocess(image)
|
||||
use_ellipse = camera.logical_name == "ir" and self.config.ir_mode in (
|
||||
"ellipse",
|
||||
"auto",
|
||||
)
|
||||
|
||||
if not use_ellipse or self.config.ir_mode == "auto":
|
||||
corners = self.detect_chessboard(proc, board_size)
|
||||
if corners is not None:
|
||||
record = FeatureRecord(
|
||||
image_path=image_path,
|
||||
json_path=json_path,
|
||||
camera_folder=camera.folder_name,
|
||||
feature_type="chessboard",
|
||||
success=True,
|
||||
board_size=board_size,
|
||||
square_size=square_size,
|
||||
corners=corners,
|
||||
preprocessing=self.config.preprocessing,
|
||||
timestamp_sec=base.timestamp_sec,
|
||||
pair_key=base.pair_key,
|
||||
)
|
||||
save_feature_json(record)
|
||||
if self.config.troubleshooting:
|
||||
self._save_corner_overlay(image, record, board_size)
|
||||
return record
|
||||
|
||||
if use_ellipse:
|
||||
result = self.detect_ellipse(image)
|
||||
if result is not None:
|
||||
(cx, cy), ellipse = result
|
||||
record = FeatureRecord(
|
||||
image_path=image_path,
|
||||
json_path=json_path,
|
||||
camera_folder=camera.folder_name,
|
||||
feature_type="ellipse",
|
||||
success=True,
|
||||
center=(cx, cy),
|
||||
ellipse=ellipse,
|
||||
preprocessing=self.config.preprocessing,
|
||||
timestamp_sec=base.timestamp_sec,
|
||||
pair_key=base.pair_key,
|
||||
)
|
||||
save_feature_json(record)
|
||||
if self.config.troubleshooting:
|
||||
self._save_corner_overlay(image, record, board_size)
|
||||
return record
|
||||
|
||||
base.feature_type = "chessboard" if not use_ellipse else "ellipse"
|
||||
base.error = "no features detected"
|
||||
save_feature_json(base)
|
||||
if self.config.troubleshooting:
|
||||
print(f"[detect] FAIL {image_path.name}: {base.error}")
|
||||
return base
|
||||
|
||||
def process_camera(
|
||||
self,
|
||||
camera: CameraFolder,
|
||||
board_size: Optional[Tuple[int, int]] = None,
|
||||
square_size: Optional[float] = None,
|
||||
) -> Tuple[int, int]:
|
||||
images = list_image_paths(camera.path)
|
||||
if not images:
|
||||
print(f"[WARN] No images in {camera.path}")
|
||||
return 0, 0
|
||||
|
||||
detected = 0
|
||||
iterator = (
|
||||
tqdm(images, unit="img", dynamic_ncols=True)
|
||||
if self.config.troubleshooting
|
||||
else images
|
||||
)
|
||||
for image_path in iterator:
|
||||
record = self.process_image(
|
||||
image_path, camera, board_size=board_size, square_size=square_size
|
||||
)
|
||||
if record.success:
|
||||
detected += 1
|
||||
if self.config.troubleshooting and hasattr(iterator, "set_description"):
|
||||
iterator.set_description(
|
||||
f"{camera.logical_name} | detected {detected}/{len(images)}"
|
||||
)
|
||||
print(f"[{camera.logical_name}] {detected}/{len(images)} features detected")
|
||||
return detected, len(images)
|
||||
|
||||
|
||||
def run_detection(
|
||||
input_path: str | Path,
|
||||
config: DetectionConfig,
|
||||
cameras: Optional[list[str]] = None,
|
||||
per_camera_board: Optional[dict] = None,
|
||||
) -> None:
|
||||
session_root = resolve_session_root(input_path)
|
||||
present = list_cameras_present(session_root)
|
||||
if cameras:
|
||||
wanted = set(cameras)
|
||||
present = [c for c in present if c.logical_name in wanted]
|
||||
|
||||
if not present:
|
||||
raise FileNotFoundError(f"No camera folders found under {session_root}")
|
||||
|
||||
corners_root = None
|
||||
if config.troubleshooting:
|
||||
corners_root = Path(input_path) / "corners"
|
||||
print(f"[detect] troubleshooting: corner overlays → {corners_root}")
|
||||
|
||||
detector = FeatureDetector(config, corners_root=corners_root)
|
||||
per_camera_board = per_camera_board or {}
|
||||
|
||||
for camera in present:
|
||||
board = per_camera_board.get(camera.logical_name, {}).get("board_size")
|
||||
square = per_camera_board.get(camera.logical_name, {}).get("square_size")
|
||||
detector.process_camera(camera, board_size=board, square_size=square)
|
||||
@@ -0,0 +1,136 @@
|
||||
"""JSON schema for per-image feature detection results."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
FEATURE_JSON_VERSION = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class FeatureRecord:
|
||||
image_path: Path
|
||||
json_path: Path
|
||||
camera_folder: str
|
||||
feature_type: str
|
||||
success: bool
|
||||
board_size: Optional[Tuple[int, int]] = None
|
||||
square_size: Optional[float] = None
|
||||
corners: Optional[np.ndarray] = None # Nx1x2 float32
|
||||
center: Optional[Tuple[float, float]] = None
|
||||
ellipse: Optional[Dict[str, Any]] = None
|
||||
timestamp_sec: Optional[float] = None
|
||||
pair_key: Optional[str] = None
|
||||
preprocessing: Optional[str] = None
|
||||
error: Optional[str] = None
|
||||
|
||||
@property
|
||||
def is_chessboard(self) -> bool:
|
||||
return self.success and self.feature_type == "chessboard" and self.corners is not None
|
||||
|
||||
@property
|
||||
def corner_count(self) -> int:
|
||||
if self.corners is None:
|
||||
return 0
|
||||
return int(self.corners.shape[0])
|
||||
|
||||
|
||||
def corners_to_list(corners: np.ndarray) -> List[List[float]]:
|
||||
flat = corners.reshape(-1, 2)
|
||||
return [[float(x), float(y)] for x, y in flat]
|
||||
|
||||
|
||||
def corners_from_list(data: List[List[float]]) -> np.ndarray:
|
||||
arr = np.array(data, dtype=np.float32).reshape(-1, 1, 2)
|
||||
return arr
|
||||
|
||||
|
||||
def save_feature_json(record: FeatureRecord) -> None:
|
||||
payload: Dict[str, Any] = {
|
||||
"version": FEATURE_JSON_VERSION,
|
||||
"image": record.image_path.name,
|
||||
"camera_folder": record.camera_folder,
|
||||
"feature_type": record.feature_type,
|
||||
"success": record.success,
|
||||
"preprocessing": record.preprocessing,
|
||||
"timestamp_sec": record.timestamp_sec,
|
||||
"pair_key": record.pair_key,
|
||||
}
|
||||
if record.board_size is not None:
|
||||
payload["board_size"] = [int(record.board_size[0]), int(record.board_size[1])]
|
||||
if record.square_size is not None:
|
||||
payload["square_size"] = float(record.square_size)
|
||||
if record.corners is not None:
|
||||
payload["corners"] = corners_to_list(record.corners)
|
||||
if record.center is not None:
|
||||
payload["center"] = [float(record.center[0]), float(record.center[1])]
|
||||
if record.ellipse is not None:
|
||||
payload["ellipse"] = record.ellipse
|
||||
if record.error:
|
||||
payload["error"] = record.error
|
||||
|
||||
record.json_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with open(record.json_path, "w", encoding="utf-8") as f:
|
||||
json.dump(payload, f, indent=2)
|
||||
|
||||
|
||||
def load_feature_json(json_path: Path, image_path: Optional[Path] = None) -> FeatureRecord:
|
||||
with open(json_path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
|
||||
if image_path is not None:
|
||||
img = Path(image_path)
|
||||
else:
|
||||
stem = json_path.stem
|
||||
parent = json_path.parent
|
||||
img = parent / data.get("image", stem)
|
||||
if not img.exists():
|
||||
for ext in (".bmp", ".png", ".jpg", ".jpeg"):
|
||||
candidate = parent / f"{stem}{ext}"
|
||||
if candidate.exists():
|
||||
img = candidate
|
||||
break
|
||||
|
||||
board_size = None
|
||||
if "board_size" in data and data["board_size"]:
|
||||
board_size = (int(data["board_size"][0]), int(data["board_size"][1]))
|
||||
|
||||
corners = None
|
||||
if data.get("corners"):
|
||||
corners = corners_from_list(data["corners"])
|
||||
|
||||
center = None
|
||||
if data.get("center"):
|
||||
center = (float(data["center"][0]), float(data["center"][1]))
|
||||
|
||||
return FeatureRecord(
|
||||
image_path=Path(img),
|
||||
json_path=Path(json_path),
|
||||
camera_folder=data.get("camera_folder", ""),
|
||||
feature_type=data.get("feature_type", "unknown"),
|
||||
success=bool(data.get("success", False)),
|
||||
board_size=board_size,
|
||||
square_size=data.get("square_size"),
|
||||
corners=corners,
|
||||
center=center,
|
||||
ellipse=data.get("ellipse"),
|
||||
timestamp_sec=data.get("timestamp_sec"),
|
||||
pair_key=data.get("pair_key"),
|
||||
preprocessing=data.get("preprocessing"),
|
||||
error=data.get("error"),
|
||||
)
|
||||
|
||||
|
||||
def load_folder_features(camera_dir: Path) -> List[FeatureRecord]:
|
||||
records = []
|
||||
for json_path in sorted(camera_dir.glob("*.json")):
|
||||
try:
|
||||
records.append(load_feature_json(json_path))
|
||||
except (json.JSONDecodeError, OSError) as exc:
|
||||
print(f"[WARN] Skipping invalid JSON {json_path}: {exc}")
|
||||
return records
|
||||
@@ -0,0 +1,106 @@
|
||||
"""Stereo pair building: time-window matching with filename-key fallback."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
from calibrationclasses.feature_json import FeatureRecord
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class StereoPair:
|
||||
left: FeatureRecord
|
||||
right: FeatureRecord
|
||||
delta_sec: float
|
||||
method: str # "time_window" | "pair_key"
|
||||
|
||||
|
||||
def _chessboard_compatible(left: FeatureRecord, right: FeatureRecord) -> bool:
|
||||
if not left.is_chessboard or not right.is_chessboard:
|
||||
return False
|
||||
return left.corner_count == right.corner_count
|
||||
|
||||
|
||||
def pair_by_time_window(
|
||||
left_records: List[FeatureRecord],
|
||||
right_records: List[FeatureRecord],
|
||||
window_sec: float,
|
||||
) -> List[StereoPair]:
|
||||
"""Match each left image to the closest unused right image within window_sec."""
|
||||
pairs: List[StereoPair] = []
|
||||
used_right: set[int] = set()
|
||||
|
||||
left_sorted = sorted(
|
||||
[r for r in left_records if r.is_chessboard and r.timestamp_sec is not None],
|
||||
key=lambda r: r.timestamp_sec,
|
||||
)
|
||||
right_candidates = [
|
||||
(i, r)
|
||||
for i, r in enumerate(right_records)
|
||||
if r.is_chessboard and r.timestamp_sec is not None
|
||||
]
|
||||
|
||||
for left in left_sorted:
|
||||
best_idx = None
|
||||
best_dt = None
|
||||
for idx, right in right_candidates:
|
||||
if idx in used_right:
|
||||
continue
|
||||
if not _chessboard_compatible(left, right):
|
||||
continue
|
||||
dt = abs(left.timestamp_sec - right.timestamp_sec)
|
||||
if dt <= window_sec and (best_dt is None or dt < best_dt):
|
||||
best_idx = idx
|
||||
best_dt = dt
|
||||
if best_idx is not None:
|
||||
used_right.add(best_idx)
|
||||
right = right_candidates[best_idx][1]
|
||||
pairs.append(StereoPair(left, right, best_dt, "time_window"))
|
||||
|
||||
return pairs
|
||||
|
||||
|
||||
def pair_by_key(
|
||||
left_records: List[FeatureRecord],
|
||||
right_records: List[FeatureRecord],
|
||||
) -> List[StereoPair]:
|
||||
"""Legacy exact pair_key matching (IR scan ids, shared numeric suffix)."""
|
||||
right_lookup: Dict[str, FeatureRecord] = {}
|
||||
for right in right_records:
|
||||
if right.is_chessboard and right.pair_key:
|
||||
right_lookup[right.pair_key] = right
|
||||
|
||||
pairs: List[StereoPair] = []
|
||||
used_right: set[str] = set()
|
||||
for left in left_records:
|
||||
if not left.is_chessboard or not left.pair_key:
|
||||
continue
|
||||
right = right_lookup.get(left.pair_key)
|
||||
if right is None or left.pair_key in used_right:
|
||||
continue
|
||||
if not _chessboard_compatible(left, right):
|
||||
continue
|
||||
used_right.add(left.pair_key)
|
||||
pairs.append(StereoPair(left, right, 0.0, "pair_key"))
|
||||
|
||||
return pairs
|
||||
|
||||
|
||||
def build_stereo_pairs(
|
||||
left_records: List[FeatureRecord],
|
||||
right_records: List[FeatureRecord],
|
||||
time_window_sec: float = 0.1,
|
||||
) -> List[StereoPair]:
|
||||
"""
|
||||
Prefer time-window pairs; fill remaining with pair_key matches not already paired.
|
||||
"""
|
||||
time_pairs = pair_by_time_window(left_records, right_records, time_window_sec)
|
||||
paired_left = {p.left.image_path for p in time_pairs}
|
||||
paired_right = {p.right.image_path for p in time_pairs}
|
||||
|
||||
remaining_left = [r for r in left_records if r.image_path not in paired_left]
|
||||
remaining_right = [r for r in right_records if r.image_path not in paired_right]
|
||||
key_pairs = pair_by_key(remaining_left, remaining_right)
|
||||
|
||||
return time_pairs + key_pairs
|
||||
@@ -0,0 +1,82 @@
|
||||
from typing import List, Tuple
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Preprocessing:
|
||||
"""Preprocessing class.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
clipLimit: float
|
||||
default = 5.0
|
||||
tileGridSize: Tuple[int, int]
|
||||
default = (15, 15)
|
||||
thresh1: int
|
||||
default = 0
|
||||
thresh2: int
|
||||
default = 255
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
tileGridSize: Tuple[int, int] = (15, 15),
|
||||
clipLimit: float = 5.0,
|
||||
thresh1: int = 0,
|
||||
thresh2: int = 255,
|
||||
) -> None:
|
||||
self.tileGridSize = tileGridSize
|
||||
self.clipLimit = clipLimit
|
||||
self.thresh1 = thresh1
|
||||
self.thresh2 = thresh2
|
||||
|
||||
|
||||
def gray(self, image: np.ndarray) -> np.ndarray:
|
||||
"""Convert to GRAY for a given image.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
image : np.ndarray
|
||||
image of chessboard
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.ndarray
|
||||
image of chessboard converted to GRAY
|
||||
"""
|
||||
gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
|
||||
return gray
|
||||
|
||||
def clahe(self, image: np.ndarray) -> np.ndarray:
|
||||
"""Apply Clahe to GRAY Shimage.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
image : np.ndarray
|
||||
image of chessboard
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.ndarray
|
||||
image of chessboard converted to GRAY and applied CLAHE
|
||||
"""
|
||||
clahe = cv2.createCLAHE(clipLimit = self.clipLimit, tileGridSize = self.tileGridSize)
|
||||
clahed = clahe.apply(self.gray(image))
|
||||
return clahed
|
||||
|
||||
def threshold(self, image: np.ndarray) -> np.ndarray:
|
||||
"""Apply Clahe to GRAY Shimage.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
image : np.ndarray
|
||||
image of chessboard
|
||||
|
||||
Returns
|
||||
-------
|
||||
np.ndarray
|
||||
image of chessboard converted to GRAY applied CLAHE and applied THRESHOLD
|
||||
"""
|
||||
criteria = cv2.THRESH_BINARY + cv2.THRESH_OTSU+1
|
||||
ret, threshold = cv2.threshold(self.clahe(image), self.thresh1, self.thresh2, criteria)
|
||||
return threshold
|
||||
@@ -0,0 +1,72 @@
|
||||
"""Calibration session path resolution and camera folder discovery."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
IMAGE_EXTENSIONS = (".bmp", ".png", ".jpg", ".jpeg")
|
||||
|
||||
# Logical camera name -> folder aliases on disk
|
||||
CAMERA_FOLDER_ALIASES: Dict[str, Tuple[str, ...]] = {
|
||||
"lc": ("lc",),
|
||||
"lc-ir": ("lc-ir", "lc_ir", "LC-IR"),
|
||||
"rc": ("rc",),
|
||||
"rg": ("rg", "rgb"),
|
||||
"ir": ("ir", "IR"),
|
||||
}
|
||||
|
||||
STEREO_PARTNERS = ("rc", "rg", "ir")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraFolder:
|
||||
logical_name: str
|
||||
path: Path
|
||||
folder_name: str
|
||||
|
||||
|
||||
def resolve_session_root(input_path: str | Path) -> Path:
|
||||
"""Return flat or nested `images/` root containing camera folders."""
|
||||
input_path = Path(input_path)
|
||||
images_dir = input_path / "images"
|
||||
if images_dir.is_dir():
|
||||
return images_dir
|
||||
return input_path
|
||||
|
||||
|
||||
def discover_camera_folder(
|
||||
session_root: Path, logical_name: str
|
||||
) -> Optional[CameraFolder]:
|
||||
aliases = CAMERA_FOLDER_ALIASES.get(logical_name)
|
||||
if not aliases:
|
||||
return None
|
||||
for folder in aliases:
|
||||
path = session_root / folder
|
||||
if path.is_dir():
|
||||
return CameraFolder(logical_name, path, folder)
|
||||
return None
|
||||
|
||||
|
||||
def list_image_paths(camera_dir: Path) -> List[Path]:
|
||||
paths = [
|
||||
camera_dir / name
|
||||
for name in os.listdir(camera_dir)
|
||||
if name.lower().endswith(IMAGE_EXTENSIONS)
|
||||
]
|
||||
return sorted(paths)
|
||||
|
||||
|
||||
def json_path_for_image(image_path: Path) -> Path:
|
||||
return image_path.with_suffix(".json")
|
||||
|
||||
|
||||
def list_cameras_present(session_root: Path) -> List[CameraFolder]:
|
||||
found = []
|
||||
for logical in CAMERA_FOLDER_ALIASES:
|
||||
cam = discover_camera_folder(session_root, logical)
|
||||
if cam is not None:
|
||||
found.append(cam)
|
||||
return found
|
||||
@@ -0,0 +1,80 @@
|
||||
"""Parse timestamps and pairing keys from calibration image filenames."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from pathlib import Path
|
||||
from typing import Optional, Tuple
|
||||
|
||||
_TS_TOKEN = re.compile(r"ts(\d+)", re.IGNORECASE)
|
||||
_SCAN_TOKEN = re.compile(r"scan(\d{6})", re.IGNORECASE)
|
||||
_IR_SCAN = re.compile(r"^ir_scan_(\d+)", re.IGNORECASE)
|
||||
|
||||
|
||||
def _digits_after_prefix(name: str, prefixes: Tuple[str, ...]) -> Optional[str]:
|
||||
lower = name.lower()
|
||||
for prefix in sorted(prefixes, key=len, reverse=True):
|
||||
if lower.startswith(prefix):
|
||||
remainder = lower[len(prefix) :].lstrip("_-.")
|
||||
m = re.match(r"(\d+)", remainder)
|
||||
if m:
|
||||
return m.group(1)
|
||||
return None
|
||||
|
||||
|
||||
def parse_timestamp_sec(filename: str) -> Optional[float]:
|
||||
"""
|
||||
Normalize filename timestamps to seconds for time-window pairing.
|
||||
|
||||
Supports:
|
||||
- lc_ts1634840093_ck.... -> ms since epoch
|
||||
- lc_1778599872850705.bmp -> µs since epoch (16+ digits)
|
||||
- lc_1778599872850.bmp -> ms (13 digits)
|
||||
"""
|
||||
name = Path(filename).name
|
||||
m = _TS_TOKEN.search(name)
|
||||
if m:
|
||||
digits = m.group(1)
|
||||
if len(digits) >= 16:
|
||||
return int(digits) / 1_000_000.0
|
||||
if len(digits) >= 13:
|
||||
return int(digits) / 1_000.0
|
||||
return int(digits) / 1_000.0
|
||||
|
||||
prefixes = ("lc-ir", "lcir", "lc_ir", "lc", "rc", "rg", "rgb", "ir")
|
||||
digits = _digits_after_prefix(name, prefixes)
|
||||
if digits is None:
|
||||
return None
|
||||
|
||||
if len(digits) >= 16:
|
||||
return int(digits) / 1_000_000.0
|
||||
if len(digits) >= 13:
|
||||
return int(digits) / 1_000.0
|
||||
return float(digits)
|
||||
|
||||
|
||||
def parse_pair_key(filename: str) -> Optional[str]:
|
||||
"""
|
||||
Filename key for legacy exact matching (IR scan ids, shared numeric tails).
|
||||
"""
|
||||
name = Path(filename).name
|
||||
lower = name.lower()
|
||||
|
||||
m = _IR_SCAN.match(lower)
|
||||
if m:
|
||||
return f"scan{int(m.group(1)):06d}"
|
||||
|
||||
m = _SCAN_TOKEN.search(lower)
|
||||
if m:
|
||||
return m.group(0).lower()
|
||||
|
||||
m = _TS_TOKEN.search(lower)
|
||||
if m:
|
||||
return f"ts{m.group(1)}"
|
||||
|
||||
prefixes = ("lc-ir", "lcir", "lc_ir", "lc", "rc", "rg", "rgb", "ir")
|
||||
digits = _digits_after_prefix(lower, prefixes)
|
||||
if digits:
|
||||
return digits
|
||||
|
||||
return Path(lower).stem
|
||||
Reference in New Issue
Block a user