jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- refactored calibration into separate class

Browse Source 
- load/store camera calibration for physical camera using device name
- load/store camera calibration for synthetic images using project name
This commit is contained in:
jens
2025-12-10 18:51:30 +01:00
parent e68507ad90
commit 51ffd9c1ee
5 changed files with 91 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/calibration.py
+56
View File
@@ -0,0 +1,56 @@
import numpy as np
import cv2 as cv
class Calibration:
def __init__(self):
self.mtx = None
self.dist = None
self.obj_points_list = []
self.img_points_list = []
def calibration_clear(self):
self.mtx = None
self.dist = None
self.obj_points_list = []
self.img_points_list = []
def is_calibrated(self):
return self.mtx is not None and self.dist is not None
def get(self):
return self.mtx, self.dist
def set(self, mtx, dist):
self.mtx = mtx
self.dist = dist
def load(self, filename: str):
try:
with np.load(filename) as X:
mtx, dist = [X[i] for i in ('mtx', 'dist')]
self.set(mtx, dist)
print(f"Loaded {filename} successfully!")
except FileNotFoundError:
pass
except KeyError:
pass
def save(self, filename: str):
mtx, dist = self.get()
np.savez(f"{filename}", mtx=mtx, dist=dist)
calib = {'mtx': mtx.tolist(), 'dist': dist.tolist()}
print(f"Saved {filename} successfully!")
def process(self, image: np.ndarray, r_vecs=None, t_vecs=None, flags=0):
success = False
if len(self.obj_points_list) > 0 and len(self.img_points_list) > 0:
ret, mtx, dist, r_vecs, t_vecs = cv.calibrateCamera(self.obj_points_list, self.img_points_list, image.shape, self.mtx, self.dist,
rvecs=r_vecs, tvecs=t_vecs, flags=flags)
if ret < 3:
success = True
self.mtx = mtx
self.dist = dist
return success
ar_tag_pose/detector_board.py
+5 -37
View File
@@ -1,6 +1,6 @@
import abc
import cv2 as cv
import numpy as np
from calibration import Calibration
RESULTS_FOLDER = "results"
@@ -9,11 +9,7 @@ class BoardDetector(abc.ABC):
return self.name
def __init__(self, name: str, board_id: str, n_syms: int):
self.mtx = None
self.dist = None
self.name = name
self.obj_points_list = []
self.img_points_list = []
self.board_id = board_id
# Create IDs
@@ -46,42 +42,14 @@ class BoardDetector(abc.ABC):
pass
@abc.abstractmethod
def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True) -> (bool, np.ndarray, np.ndarray):
def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True) -> (bool, np.ndarray, np.ndarray):
pass
def is_calibrated(self):
return self.mtx is not None and self.dist is not None
def collect_points(self, image: np.ndarray) -> bool:
def collect_points(self, image: np.ndarray, calibration: Calibration) -> bool:
success, obj_points, img_points = self.match_points(image)
if success:
self.obj_points_list.append(obj_points)
self.img_points_list.append(img_points)
calibration.obj_points_list.append(obj_points)
calibration.img_points_list.append(img_points)
return success
def calibration_clear(self):
self.mtx = None
self.dist = None
self.obj_points_list = []
self.img_points_list = []
def calibration_set(self, mtx, dist):
self.mtx = mtx
self.dist = dist
def calibration_get(self):
return self.mtx, self.dist
def calibrate_camera(self, image: np.ndarray, r_vecs=None, t_vecs=None, flags=0):
success = False
if len(self.obj_points_list) > 0 and len(self.img_points_list) > 0:
ret, mtx, dist, r_vecs, t_vecs = cv.calibrateCamera(self.obj_points_list, self.img_points_list, image.shape, self.mtx, self.dist,
rvecs=r_vecs, tvecs=t_vecs, flags=flags)
if ret < 3:
success = True
self.mtx = mtx
self.dist = dist
return success
ar_tag_pose/detector_board_aruco.py
+8 -6
View File
@@ -1,6 +1,8 @@
import cv2 as cv
import numpy as np
from ar_tag_pose.detector_board import BoardDetector
from ar_tag_pose.calibration import Calibration
from ar_tag_pose.aruco_types import ARUCO_DICT
def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, ids: np.array=None):
@@ -48,7 +50,7 @@ class BoardDetectorAruco(BoardDetector):
return success, obj_points, img_points
def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True):
def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True):
pose = False
r_vec = None
t_vec = None
@@ -66,15 +68,15 @@ class BoardDetectorAruco(BoardDetector):
if draw_marker_id:
cv.aruco.drawDetectedMarkers(image, corners, ids)
if not self.is_calibrated():
if not calibration.is_calibrated():
return pose, None, None
# Draw marker axis
if draw_marker_axis:
_r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=self.mtx, distCoeffs=self.dist)
_r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=calibration.mtx, distCoeffs=calibration.dist)
if _r_vecs is not None:
for i in range(len(_r_vecs)):
cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05)
cv.drawFrameAxes(image, calibration.mtx, calibration.dist, _r_vecs[i], _t_vecs[i], 0.05)
# Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients)
obj_points, img_points = self.board.matchImagePoints(corners, ids)
@@ -82,10 +84,10 @@ class BoardDetectorAruco(BoardDetector):
offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation())
offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation())
obj_points -= [offset_w/2, offset_h/2, 0]
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=None, tvec=None)
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, calibration.mtx, calibration.dist, rvec=None, tvec=None)
if pose:
# Draw Axis
cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5)
cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5)
return pose, r_vec, t_vec
ar_tag_pose/detector_board_charuco.py
+9 -7
View File
@@ -1,6 +1,8 @@
import cv2 as cv
import numpy as np
from ar_tag_pose.detector_board import BoardDetector
from ar_tag_pose.calibration import Calibration
from ar_tag_pose.aruco_types import ARUCO_DICT
def factory(dict_type: int, board_size: cv.typing.Size, square_length: float = 5.0, marker_length: float = 3.0, ids: np.array=None):
@@ -49,7 +51,7 @@ class BoardDetectorCharuco(BoardDetector):
return success, obj_points, img_points
def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True):
def process(self, image: np.ndarray, calibration: Calibration, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True):
pose = False
r_vec = None
t_vec = None
@@ -68,11 +70,11 @@ class BoardDetectorCharuco(BoardDetector):
cv.aruco.drawDetectedCornersCharuco(image, charuco_corners, charuco_ids)
# Draw marker axis
if draw_marker_axis and self.is_calibrated():
if draw_marker_axis and calibration.is_calibrated():
_r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1,
cameraMatrix=self.mtx, distCoeffs=self.dist)
cameraMatrix=calibration.mtx, distCoeffs=calibration.dist)
for i in range(len(_r_vecs)):
cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05)
cv.drawFrameAxes(image, calibration.mtx, calibration.dist, _r_vecs[i], _t_vecs[i], 0.05)
if len(charuco_ids) >= len(self.board.getIds()):
obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids)
@@ -80,12 +82,12 @@ class BoardDetectorCharuco(BoardDetector):
offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength()
offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength()
obj_points -= [offset_w/2, offset_h/2, 0]
if self.is_calibrated():
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, useExtrinsicGuess=False,
if calibration.is_calibrated():
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, calibration.mtx, calibration.dist, useExtrinsicGuess=False,
rvec=None, tvec=None, flags=cv.SOLVEPNP_ITERATIVE)
if pose:
# Draw
cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10)
cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, 10)
return pose, r_vec, t_vec
ar_tag_pose/detector_board_main.py
+13 -39
View File
@@ -7,6 +7,7 @@ import mimetypes
import json
from pathlib import Path
from ar_tag_pose.utils import to_tf, to_euler, draw_text
from calibration import Calibration
from detector_board import BoardDetector
from detector_board_aruco import BoardDetectorAruco
from detector_board_charuco import BoardDetectorCharuco
@@ -15,7 +16,8 @@ from cv2_enumerate_cameras import enumerate_cameras
RESULTS_FOLDER = "results"
class Detector(abc.ABC):
def __init__(self, params: dict, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''):
def __init__(self, name: str, params: dict, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''):
self.name = name
self.params = params
self.detector = detector
self.out_file = out_file
@@ -51,7 +53,10 @@ class Detector(abc.ABC):
elif self.src_images is not None:
# Open image file sequence
self.cap = cv.VideoCapture(self.src_images, cv.CAP_IMAGES)
self.camera_name = self.name
self.calibration = Calibration()
self.calibration.load(f"{RESULTS_FOLDER}/{self.camera_name}_cal.npz")
def params_save(self):
with open(f"{RESULTS_FOLDER}/{self.params["name"]}.json", 'w') as fp:
@@ -98,34 +103,6 @@ class Detector(abc.ABC):
name = det.get_board_name()
cv.imwrite(f"{RESULTS_FOLDER}/{name}.png", image)
def calibrate_save(self, det: BoardDetector):
filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz"
mtx, dist = det.calibration_get()
np.savez(f"{filename}", mtx=mtx, dist=dist)
calib = {'mtx': mtx.tolist(), 'dist': dist.tolist()}
self.params['calib'][self.camera_name][str(det)] = calib
self.params_save()
print(f"Saved {filename} successfully!")
def calibrate_load(self):
calib = {}
for det in self.detector:
filename = f"{RESULTS_FOLDER}/{det.get_board_name()}_cal.npz"
try:
with np.load(filename) as X:
mtx, dist = [X[i] for i in ('mtx', 'dist')]
calib[str(det)] = {'mtx': mtx.tolist(), 'dist': dist.tolist()}
det.calibration_set(mtx, dist)
self.params['calib'] = calib
self.params_save()
print(f"Loaded {filename} successfully!")
except FileNotFoundError:
pass
except KeyError:
pass
self.params['calib'] = {self.camera_name: calib}
def process(self):
cap = self.cap
if cap is None or not cap.isOpened():
@@ -162,25 +139,23 @@ class Detector(abc.ABC):
elif k == ord('p'):
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
for det in self.detector:
success = det.collect_points(gray)
success = det.collect_points(gray, self.calibration)
if success:
print(f"{det}: Points matching success")
elif k == ord('c'):
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
for det in self.detector:
if det.calibrate_camera(gray):
print(f"{det}: Camera calibration success")
self.calibrate_save(det)
if self.calibration.process(gray):
print(f"{self.name}: Camera calibration success")
self.calibration.save(f"{RESULTS_FOLDER}/{self.camera_name}_cal.npz")
elif k == ord('x'):
for det in self.detector:
det.calibration_clear()
self.calibration.calibration_clear()
elif k == ord('f'):
pose_center = not pose_center
k = 800 / img.shape[1]
y_pos = 30
for det in self.detector:
success, r_vec, t_vec = det.process(img, True, False, False, pose_center)
success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center)
if success:
if len(self.rotation_order) == 3:
rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order)
@@ -287,9 +262,8 @@ def main():
out_file = f"{var_args["name"]}.mp4"
# Create Detector app
detector = Detector(params, var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"])
detector = Detector(var_args["name"], params, var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"])
detector.board_image_generate(margin_length=var_args["margin_length"])
detector.calibrate_load()
detector.process()