1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
#!/usr/bin/env python
""" """
__author__ = "Théo de la Hogue"
__credits__ = []
__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)"
__license__ = "BSD"
from dataclasses import dataclass, field
from argaze import DataStructures
import json
import numpy
import cv2
import cv2.aruco as aruco
K0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 0.]])
"""Define default camera intrinsic parameters matrix."""
D0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0])
"""Define default camera distorsion coefficients vector."""
@dataclass
class CalibrationData():
"""Define optical camera calibration data."""
rms: float = field(default=0)
"""Root Mean Square error of calibration."""
dimensions: numpy.array = field(default_factory=lambda : numpy.array([0, 0]))
"""Frame dimensions in pixels from which the calibration have been done."""
K: numpy.array = field(default_factory=lambda : K0)
"""Intrinsic parameters matrix (focal lengths and principal point)."""
D: numpy.array = field(default_factory=lambda : D0)
"""Distorsion coefficients vector."""
@classmethod
def from_json(self, json_filepath):
"""Load optical parameters from .json file."""
with open(json_filepath) as calibration_file:
return CalibrationData(**json.load(calibration_file))
def to_json(self, json_filepath):
"""Save optical parameters into .json file."""
with open(json_filepath, 'w', encoding='utf-8') as calibration_file:
json.dump(self, calibration_file, ensure_ascii=False, indent=4, cls=DataStructures.JsonEncoder)
def __str__(self) -> str:
"""String display"""
output = f'\trms: {self.rms}\n'
output += f'\tdimensions: {self.dimensions}\n'
output += f'\tK: {self.K}\n'
output += f'\tD: {self.D}\n'
return output
def draw(self, frame: numpy.array, width:float, height:float, z:float, color=(0, 0, 255)):
"""Draw grid to display K and D"""
# Edit 3D grid
grid_3D = []
for x in range(-int(width/2), int(width/2)):
for y in range(-int(height/2), int(height/2)):
grid_3D.append([x, y, z])
# Project 3d grid
grid_2D, _ = cv2.projectPoints(numpy.array(grid_3D).astype(float), numpy.array([0., 0., 0.]), numpy.array([0., 0., 0.]), numpy.array(self.K), -numpy.array(self.D))
# Draw projection
for point in grid_2D:
# Ignore point out out field
try:
cv2.circle(frame, point.astype(int)[0], 1, color, -1)
except:
pass
class ArUcoCamera(CalibrationData):
"""Handle camera calibration process."""
def __init__(self, **kwargs):
super().__init__(**kwargs)
# Calibration data
self.__corners_set_number = 0
self.__corners_set = []
self.__corners_set_ids = []
def calibrate(self, board):
"""Retrieve camera K and D from stored calibration data."""
if self.__corners_set_number > 0:
self.rms, self.K, self.D, r, t = aruco.calibrateCameraCharuco(self.__corners_set, self.__corners_set_ids, board.model, self.dimensions, None, None)
def reset_calibration_data(self):
"""Clear all calibration data."""
self.__corners_set_number = 0
self.__corners_set = []
self.__corners_set_ids = []
def store_calibration_data(self, corners, corners_identifiers):
"""Store calibration data."""
self.__corners_set_number += 1
self.__corners_set.append(corners)
self.__corners_set_ids.append(corners_identifiers)
@property
def calibration_data_count(self) -> int:
"""Get how much calibration data are stored."""
return self.__corners_set_number
|
