From 4a5c7ad8bf29afc5af893c524e5753de302873b7 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 14 Jun 2023 11:36:40 +0200 Subject: Renaming ArUcoCamera into ArUcoOpticCalibrator. Using OpticParameters class to handle K and D parameters. --- src/argaze.test/ArFeatures.py | 10 +- src/argaze.test/ArUcoMarkers/ArUcoCamera.py | 52 -------- src/argaze.test/ArUcoMarkers/ArUcoDetector.py | 8 +- .../ArUcoMarkers/ArUcoOpticCalibrator.py | 52 ++++++++ src/argaze.test/ArUcoMarkers/utils/camera.json | 31 ----- src/argaze.test/ArUcoMarkers/utils/detector.json | 2 +- .../ArUcoMarkers/utils/optic_parameters.json | 31 +++++ src/argaze.test/utils/environment.json | 2 +- src/argaze/ArFeatures.py | 22 ++-- src/argaze/ArUcoMarkers/ArUcoCamera.py | 125 ------------------- src/argaze/ArUcoMarkers/ArUcoDetector.py | 20 ++-- src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py | 132 +++++++++++++++++++++ src/argaze/ArUcoMarkers/ArUcoScene.py | 2 +- src/argaze/ArUcoMarkers/__init__.py | 2 +- src/argaze/AreaOfInterest/AOI3DScene.py | 4 +- src/argaze/utils/camera_calibrate.py | 45 ++++--- src/argaze/utils/demo_environment/setup.json | 2 +- src/argaze/utils/demo_heatmap_run.py | 2 +- src/argaze/utils/environment_edit.py | 2 +- 19 files changed, 280 insertions(+), 266 deletions(-) delete mode 100644 src/argaze.test/ArUcoMarkers/ArUcoCamera.py create mode 100644 src/argaze.test/ArUcoMarkers/ArUcoOpticCalibrator.py delete mode 100644 src/argaze.test/ArUcoMarkers/utils/camera.json create mode 100644 src/argaze.test/ArUcoMarkers/utils/optic_parameters.json delete mode 100644 src/argaze/ArUcoMarkers/ArUcoCamera.py create mode 100644 src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py (limited to 'src') diff --git a/src/argaze.test/ArFeatures.py b/src/argaze.test/ArFeatures.py index 3f7972d..5da63d1 100644 --- a/src/argaze.test/ArFeatures.py +++ b/src/argaze.test/ArFeatures.py @@ -37,11 +37,11 @@ class TestArEnvironmentClass(unittest.TestCase): self.assertEqual(ar_environment.aruco_detector.parameters.aprilTagQuadSigma, 2) self.assertEqual(ar_environment.aruco_detector.parameters.aprilTagDeglitch, 1) - # Check ArUco detector camera - self.assertEqual(ar_environment.aruco_detector.camera.rms, 1.0) - self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.camera.dimensions, [1920, 1080])) - self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.camera.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) - self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.camera.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) + # Check ArUco detector optic parameters + self.assertEqual(ar_environment.aruco_detector.optic_parameters.rms, 1.0) + self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.optic_parameters.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.optic_parameters.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) + self.assertIsNone(numpy.testing.assert_array_equal(ar_environment.aruco_detector.optic_parameters.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) # Check environment scenes self.assertEqual(len(ar_environment.scenes), 2) diff --git a/src/argaze.test/ArUcoMarkers/ArUcoCamera.py b/src/argaze.test/ArUcoMarkers/ArUcoCamera.py deleted file mode 100644 index 7a53070..0000000 --- a/src/argaze.test/ArUcoMarkers/ArUcoCamera.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -""" """ - -__author__ = "Théo de la Hogue" -__credits__ = [] -__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" -__license__ = "BSD" - -import unittest -import os - -from argaze.ArUcoMarkers import ArUcoCamera - -import numpy - -class TestArUcoCameraClass(unittest.TestCase): - """Test ArUcoCamera class.""" - - def test_new(self): - """Test ArUcoCamera creation.""" - - # Check defaut camera creation - aruco_camera = ArUcoCamera.ArUcoCamera() - - # Check ArUco camera - self.assertEqual(aruco_camera.rms, 0.0) - - #self.assertEqual(type(aruco_camera.K), numpy.array) - - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.dimensions, [0, 0])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.K, ArUcoCamera.K0)) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.D, ArUcoCamera.D0)) - - def test_from_json(self): - - # Edit camera file path - current_directory = os.path.dirname(os.path.abspath(__file__)) - json_filepath = os.path.join(current_directory, 'utils/camera.json') - - # Load camera calibration - aruco_camera = ArUcoCamera.ArUcoCamera.from_json(json_filepath) - - # Check ArUco camera - self.assertEqual(aruco_camera.rms, 1.0) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.dimensions, [1920, 1080])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) - -if __name__ == '__main__': - - unittest.main() \ No newline at end of file diff --git a/src/argaze.test/ArUcoMarkers/ArUcoDetector.py b/src/argaze.test/ArUcoMarkers/ArUcoDetector.py index cd7c90c..750aaa5 100644 --- a/src/argaze.test/ArUcoMarkers/ArUcoDetector.py +++ b/src/argaze.test/ArUcoMarkers/ArUcoDetector.py @@ -11,7 +11,7 @@ import unittest import os import math -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoCamera, ArUcoDetector, ArUcoBoard +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoOpticCalibrator, ArUcoDetector, ArUcoBoard import cv2 as cv import numpy @@ -50,7 +50,7 @@ class TestArUcoDetectorClass(unittest.TestCase): # Check ArUcoDetector creation self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL') self.assertEqual(aruco_detector.marker_size, 3) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.optic_parameters.dimensions, [0, 0])) self.assertEqual(aruco_detector.detected_markers_number, 0) self.assertEqual(aruco_detector.detected_markers, {}) @@ -60,7 +60,7 @@ class TestArUcoDetectorClass(unittest.TestCase): # Check ArUcoDetector creation self.assertEqual(aruco_detector.dictionary.name, 'DICT_APRILTAG_16h5') self.assertEqual(aruco_detector.marker_size, 5.2) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.optic_parameters.dimensions, [0, 0])) self.assertEqual(aruco_detector.detected_markers_number, 0) self.assertEqual(aruco_detector.detected_markers, {}) @@ -77,7 +77,7 @@ class TestArUcoDetectorClass(unittest.TestCase): # Check ArUcoDetector creation self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL') self.assertEqual(aruco_detector.marker_size, 3) - self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.optic_parameters.dimensions, [1920, 1080])) self.assertEqual(aruco_detector.parameters.cornerRefinementMethod, 3) self.assertEqual(aruco_detector.parameters.aprilTagQuadSigma, 2) self.assertEqual(aruco_detector.parameters.aprilTagDeglitch, 1) diff --git a/src/argaze.test/ArUcoMarkers/ArUcoOpticCalibrator.py b/src/argaze.test/ArUcoMarkers/ArUcoOpticCalibrator.py new file mode 100644 index 0000000..45b7669 --- /dev/null +++ b/src/argaze.test/ArUcoMarkers/ArUcoOpticCalibrator.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +""" """ + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "BSD" + +import unittest +import os + +from argaze.ArUcoMarkers import ArUcoOpticCalibrator + +import numpy + +class TestOpticParametersClass(unittest.TestCase): + """Test OpticParameters class.""" + + def test_new(self): + """Test OpticParameters creation.""" + + # Check defaut optic parameters creation + optic_parameters = ArUcoOpticCalibrator.OpticParameters() + + # Check ArUco optic parameters + self.assertEqual(optic_parameters.rms, 0.0) + + #self.assertEqual(type(optic_parameters.K), numpy.array) + + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.dimensions, [0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.K, ArUcoOpticCalibrator.K0)) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.D, ArUcoOpticCalibrator.D0)) + + def test_from_json(self): + + # Edit optic parameters file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/optic_parameters.json') + + # Load optic parameters + optic_parameters = ArUcoOpticCalibrator.OpticParameters.from_json(json_filepath) + + # Check ArUco camera + self.assertEqual(optic_parameters.rms, 1.0) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.K, [[1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [0.0, 0.0, 1.0]])) + self.assertIsNone(numpy.testing.assert_array_equal(optic_parameters.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) + +if __name__ == '__main__': + + unittest.main() \ No newline at end of file diff --git a/src/argaze.test/ArUcoMarkers/utils/camera.json b/src/argaze.test/ArUcoMarkers/utils/camera.json deleted file mode 100644 index 988731c..0000000 --- a/src/argaze.test/ArUcoMarkers/utils/camera.json +++ /dev/null @@ -1,31 +0,0 @@ -{ - "rms": 1.0, - "dimensions": [ - 1920, - 1080 - ], - "K": [ - [ - 1.0, - 0.0, - 1.0 - ], - [ - 0.0, - 1.0, - 1.0 - ], - [ - 0.0, - 0.0, - 1.0 - ] - ], - "D": [ - -1.0, - -0.5, - 0.0, - 0.5, - 1.0 - ] -} \ No newline at end of file diff --git a/src/argaze.test/ArUcoMarkers/utils/detector.json b/src/argaze.test/ArUcoMarkers/utils/detector.json index a239bf8..8aada6d 100644 --- a/src/argaze.test/ArUcoMarkers/utils/detector.json +++ b/src/argaze.test/ArUcoMarkers/utils/detector.json @@ -3,7 +3,7 @@ "name": "DICT_ARUCO_ORIGINAL" }, "marker_size": 3.0, - "camera": { + "optic_parameters": { "rms": 1.0, "dimensions": [ 1920, diff --git a/src/argaze.test/ArUcoMarkers/utils/optic_parameters.json b/src/argaze.test/ArUcoMarkers/utils/optic_parameters.json new file mode 100644 index 0000000..988731c --- /dev/null +++ b/src/argaze.test/ArUcoMarkers/utils/optic_parameters.json @@ -0,0 +1,31 @@ +{ + "rms": 1.0, + "dimensions": [ + 1920, + 1080 + ], + "K": [ + [ + 1.0, + 0.0, + 1.0 + ], + [ + 0.0, + 1.0, + 1.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "D": [ + -1.0, + -0.5, + 0.0, + 0.5, + 1.0 + ] +} \ No newline at end of file diff --git a/src/argaze.test/utils/environment.json b/src/argaze.test/utils/environment.json index 57d04cf..df1c771 100644 --- a/src/argaze.test/utils/environment.json +++ b/src/argaze.test/utils/environment.json @@ -5,7 +5,7 @@ "name": "DICT_ARUCO_ORIGINAL" }, "marker_size": 3.0, - "camera": { + "optic_parameters": { "rms": 1.0, "dimensions": [ 1920, diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py index 5899545..03f57f6 100644 --- a/src/argaze/ArFeatures.py +++ b/src/argaze/ArFeatures.py @@ -65,22 +65,22 @@ class ArEnvironment(): new_aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(**new_detector_data.pop('dictionary')) new_marker_size = new_detector_data.pop('marker_size') - # Check aruco_camera value type - aruco_camera_value = new_detector_data.pop('camera') + # Check optic_parameters value type + optic_parameters_value = new_detector_data.pop('optic_parameters') # str: relative path to .json file - if type(aruco_camera_value) == str: + if type(optic_parameters_value) == str: - aruco_camera_value = os.path.join(working_directory, aruco_camera_value) - new_aruco_camera = ArUcoCamera.ArUcoCamera.from_json(aruco_camera_value) + optic_parameters_value = os.path.join(working_directory, optic_parameters_value) + new_optic_parameters = ArUcoOpticCalibrator.OpticParameters.from_json(optic_parameters_value) # dict: else: - new_aruco_camera = ArUcoCamera.ArUcoCamera(**aruco_camera_value) + new_optic_parameters = ArUcoOpticCalibrator.OpticParameters(**optic_parameters_value) new_aruco_detecor_parameters = ArUcoDetector.DetectorParameters(**new_detector_data.pop('parameters')) - new_aruco_detector = ArUcoDetector.ArUcoDetector(new_aruco_dictionary, new_marker_size, new_aruco_camera, new_aruco_detecor_parameters) + new_aruco_detector = ArUcoDetector.ArUcoDetector(new_aruco_dictionary, new_marker_size, new_optic_parameters, new_aruco_detecor_parameters) new_scenes = {} for scene_name, scene_data in data.pop('scenes').items(): @@ -223,7 +223,7 @@ class ArScene(): tvec = self.aoi_scene.center*[-1, 1, 0] + [0, 0, scene_size[1]] rvec = numpy.array([[-numpy.pi, 0.0, 0.0]]) - # Edit intrinsic camera parameter to capture whole scene + # Edit optic intrinsic parameter to capture whole scene K = numpy.array([[scene_size[1]/scene_size[0], 0.0, 0.5], [0.0, 1., 0.5], [0.0, 0.0, 1.0]]) return self.aoi_scene.project(tvec, rvec, K) @@ -319,7 +319,7 @@ class ArScene(): aoi_scene_copy = self.aoi_scene.copy() - aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self._environment.aruco_detector.camera.K) + aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self._environment.aruco_detector.optic_parameters.K) # Warn user when the projected scene is empty if len(aoi_scene_projection) == 0: @@ -379,7 +379,7 @@ class ArScene(): frame: where to draw """ - self.aruco_scene.draw_axis(frame, self._environment.aruco_detector.camera.K, self._environment.aruco_detector.camera.D) + self.aruco_scene.draw_axis(frame, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D) def draw_places(self, frame: numpy.array): """ @@ -389,6 +389,6 @@ class ArScene(): frame: where to draw """ - self.aruco_scene.draw_places(frame, self._environment.aruco_detector.camera.K, self._environment.aruco_detector.camera.D) + self.aruco_scene.draw_places(frame, self._environment.aruco_detector.optic_parameters.K, self._environment.aruco_detector.optic_parameters.D) diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py deleted file mode 100644 index 205c591..0000000 --- a/src/argaze/ArUcoMarkers/ArUcoCamera.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/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 - diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py index 4392e7a..5891f16 100644 --- a/src/argaze/ArUcoMarkers/ArUcoDetector.py +++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py @@ -12,7 +12,7 @@ from dataclasses import dataclass, field import json from collections import Counter -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoCamera +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator import numpy import cv2 as cv @@ -24,7 +24,7 @@ ArUcoMarkerDictionaryType = TypeVar('ArUcoMarkerDictionary', bound="ArUcoMarkerD ArUcoMarkerType = TypeVar('ArUcoMarker', bound="ArUcoMarker") # Type definition for type annotation convenience -ArUcoCameraType = TypeVar('ArUcoCamera', bound="ArUcoCamera") +OpticParametersType = TypeVar('OpticParameters', bound="OpticParameters") # Type definition for type annotation convenience DetectorParametersType = TypeVar('DetectorParameters', bound="DetectorParameters") @@ -127,8 +127,8 @@ class ArUcoDetector(): marker_size: float = field(default=0.) """Size of ArUco markers to detect in centimeter.""" - camera: ArUcoCamera.ArUcoCamera = field(default_factory=ArUcoCamera.ArUcoCamera) - """ArUco camera to use to setup optical parameters.""" + optic_parameters: ArUcoOpticCalibrator.OpticParameters = field(default_factory=ArUcoOpticCalibrator.OpticParameters) + """Optic parameters to use for ArUco detection into frame.""" parameters: DetectorParameters = field(default_factory=DetectorParameters) """ArUco detector parameters.""" @@ -160,18 +160,18 @@ class ArUcoDetector(): new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(**data.pop('dictionary')) new_marker_size = data.pop('marker_size') - new_camera = ArUcoCamera.ArUcoCamera(**data.pop('camera')) + new_optic_parameters = ArUcoOpticCalibrator.OpticParameters(**data.pop('optic_parameters')) new_parameters = DetectorParameters(**data.pop('parameters')) - return ArUcoDetector(new_dictionary, new_marker_size, new_camera, new_parameters) + return ArUcoDetector(new_dictionary, new_marker_size, new_optic_parameters, new_parameters) def __str__(self) -> str: """String display""" output = f'\n\tDictionary: {self.dictionary}\n' output += f'\tMarker size: {self.marker_size} cm\n\n' - output += f'\tCamera:\n{self.camera}\n' - output += f'\tParameters:\n{self.parameters}' + output += f'\tOptic parameters:\n{self.optic_parameters}\n' + output += f'\tDetection Parameters:\n{self.parameters}' return output @@ -240,7 +240,7 @@ class ArUcoDetector(): # Estimate pose of selected markers if len(selected_markers_corners) > 0: - markers_rvecs, markers_tvecs, markers_points = aruco.estimatePoseSingleMarkers(selected_markers_corners, self.marker_size, numpy.array(self.camera.K), numpy.array(self.camera.D)) + markers_rvecs, markers_tvecs, markers_points = aruco.estimatePoseSingleMarkers(selected_markers_corners, self.marker_size, numpy.array(self.optic_parameters.K), numpy.array(self.optic_parameters.D)) for i, marker_id in enumerate(selected_markers_ids): @@ -267,7 +267,7 @@ class ArUcoDetector(): for marker_id, marker in self.__detected_markers.items(): - marker.draw(frame, self.camera.K, self.camera.D) + marker.draw(frame, self.optic_parameters.K, self.optic_parameters.D) def detect_board(self, frame: numpy.array, board, expected_markers_number): """Detect ArUco markers board in frame setting up the number of detected markers needed to agree detection. diff --git a/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py b/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py new file mode 100644 index 0000000..ed33c95 --- /dev/null +++ b/src/argaze/ArUcoMarkers/ArUcoOpticCalibrator.py @@ -0,0 +1,132 @@ +#!/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 optic intrinsic parameters matrix.""" + +D0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0]) +"""Define default optic distorsion coefficients vector.""" + +@dataclass +class OpticParameters(): + """Define optic parameters outputed by optic calibrator.""" + + 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 OpticParameters(**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 ArUcoOpticCalibrator(): + """Handle optic calibration process.""" + + def __init__(self,): + + # Calibration data + self.__corners_set_number = 0 + self.__corners_set = [] + self.__corners_set_ids = [] + + def calibrate(self, board, dimensions:tuple = (0, 0)) -> OpticParameters: + """Retrieve K and D parameters from stored calibration data. + + Parameters: + dimensions: camera frame dimensions + + Returns: + Optic parameters + """ + + if self.__corners_set_number > 0: + + rms, K, D, r, t = aruco.calibrateCameraCharuco(self.__corners_set, self.__corners_set_ids, board.model, dimensions, None, None) + + return OpticParameters(rms, dimensions, K, D) + + 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 + diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 672e31c..85c3fbf 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -14,7 +14,7 @@ import math import itertools import re -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoCamera +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator import numpy import cv2 as cv diff --git a/src/argaze/ArUcoMarkers/__init__.py b/src/argaze/ArUcoMarkers/__init__.py index ac1bee4..350c69e 100644 --- a/src/argaze/ArUcoMarkers/__init__.py +++ b/src/argaze/ArUcoMarkers/__init__.py @@ -1,4 +1,4 @@ """ Handle [OpenCV ArUco markers](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html): generate and detect markers, calibrate camera, describe scene, ... """ -__all__ = ['ArUcoMarkersDictionary', 'ArUcoMarker', 'ArUcoBoard', 'ArUcoCamera', 'ArUcoDetector', 'ArUcoScene', 'utils'] \ No newline at end of file +__all__ = ['ArUcoMarkersDictionary', 'ArUcoMarker', 'ArUcoBoard', 'ArUcoOpticCalibrator', 'ArUcoDetector', 'ArUcoScene', 'utils'] \ No newline at end of file diff --git a/src/argaze/AreaOfInterest/AOI3DScene.py b/src/argaze/AreaOfInterest/AOI3DScene.py index dbb0424..2272206 100644 --- a/src/argaze/AreaOfInterest/AOI3DScene.py +++ b/src/argaze/AreaOfInterest/AOI3DScene.py @@ -24,10 +24,10 @@ R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) """Define no rotation matrix.""" K0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 0.]]) -"""Define default camera intrinsic parameters matrix.""" +"""Define default optic intrinsic parameters matrix.""" D0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0]) -"""Define default camera distorsion coefficients vector.""" +"""Define default optic distorsion coefficients vector.""" AOI3DSceneType = TypeVar('AOI3DScene', bound="AOI3DScene") # Type definition for type annotation convenience diff --git a/src/argaze/utils/camera_calibrate.py b/src/argaze/utils/camera_calibrate.py index b590767..c42b721 100644 --- a/src/argaze/utils/camera_calibrate.py +++ b/src/argaze/utils/camera_calibrate.py @@ -11,23 +11,23 @@ import argparse import os import time -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoBoard, ArUcoDetector, ArUcoCamera +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoBoard, ArUcoDetector, ArUcoOpticCalibrator import cv2 def main(): """ - Captures board pictures and finally outputs camera calibration data into a calibration.json file. + Captures board pictures and finally outputs optic parameter into a calibration.json file. - Export and print a calibration board using aruco_calibration_board_export.py script. - Place the calibration board face to the camera. - Move the calibration board in a manner to view it entirely on screen in various configurations (orientation and distance): The script will automatically take pictures. Do this step with a good lighting and a clear background. - - Once enough pictures have been captured (~20), press Esc key then, wait for the camera calibration processing. + - Once enough pictures have been captured (~20), press Esc key then, wait for optic parameters processing. - Finally, check rms parameter: it should be between 0. and 1. if the calibration succeeded (lower is better). ### Reference: - - [Camera calibration using ArUco marker tutorial](https://automaticaddison.com/how-to-perform-camera-calibration-using-opencv/) + - [Optic calibration using ArUco marker tutorial](https://automaticaddison.com/how-to-perform-camera-calibration-using-opencv/) """ # Manage arguments @@ -48,8 +48,8 @@ def main(): frame_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH)) frame_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT)) - # Create aruco camera - aruco_camera = ArUcoCamera.ArUcoCamera(dimensions=(frame_width, frame_height)) + # Create aruco optic calibrator + aruco_optic_calibrator = ArUcoOpticCalibrator.ArUcoOpticCalibrator() # Create aruco board aruco_board = ArUcoBoard.ArUcoBoard(args.columns, args.rows, args.square_size, args.marker_size, args.dictionary) @@ -57,7 +57,7 @@ def main(): # Create aruco detector aruco_detector = ArUcoDetector.ArUcoDetector(dictionary=args.dictionary, marker_size=args.marker_size) - print(f'{aruco_camera.dimensions[0]}x{aruco_camera.dimensions[1]} pixels camera calibration starts') + print(f'{frame_width}x{frame_height} pixels camera calibration starts') print("Waiting for calibration board...") expected_markers_number = aruco_board.markers_number @@ -80,8 +80,8 @@ def main(): aruco_detector.draw_detected_markers(video_frame) # Draw current calibration data count - cv2.putText(video_frame, f'Capture: {aruco_camera.calibration_data_count}', (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA) - cv2.imshow('Camera Calibration', video_frame) + cv2.putText(video_frame, f'Capture: {aruco_optic_calibrator.calibration_data_count}', (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA) + cv2.imshow('Optic Calibration', video_frame) # If all board corners are detected if aruco_detector.board_corners_number == expected_corners_number: @@ -90,9 +90,9 @@ def main(): aruco_detector.draw_board(video_frame) # Append calibration data - aruco_camera.store_calibration_data(aruco_detector.board_corners, aruco_detector.board_corners_identifier) + aruco_optic_calibrator.store_calibration_data(aruco_detector.board_corners, aruco_detector.board_corners_identifier) - cv2.imshow('Camera Calibration', video_frame) + cv2.imshow('Optic Calibration', video_frame) # Stop calibration by pressing 'Esc' key if cv2.waitKey(1) == 27: @@ -106,17 +106,24 @@ def main(): cv2.destroyAllWindows() print('\nCalibrating camera...') - aruco_camera.calibrate(aruco_board) + optic_parameters = aruco_optic_calibrator.calibrate(aruco_board, dimensions=(frame_width, frame_height)) - print('\nCalibration succeeded!') - print(f'\nRMS:\n{aruco_camera.rms}') - print(f'\nDimensions:\n{frame_width}x{frame_height}') - print(f'\nCamera matrix:\n{aruco_camera.K}') - print(f'\nDistortion coefficients:\n{aruco_camera.D}') + if optic_parameters: - aruco_camera.to_json(f'{args.output}/calibration.json') + print('\nCalibration succeeded!') - print(f'\ncalibration.json file exported into {args.output} folder') + print(f'\nRMS:\n{optic_parameters.rms}') + print(f'\nDimensions:\n{optic_parameters.dimensions[0]}x{optic_parameters.dimensions[1]}') + print(f'\nCamera matrix:\n{optic_parameters.K}') + print(f'\nDistortion coefficients:\n{optic_parameters.D}') + + optic_parameters.to_json(f'{args.output}/optic_parameters.json') + + print(f'\ncalibration.json file exported into {args.output} folder') + + else: + + print('\nCalibration error.') if __name__ == '__main__': diff --git a/src/argaze/utils/demo_environment/setup.json b/src/argaze/utils/demo_environment/setup.json index d67b53f..58c7c0d 100644 --- a/src/argaze/utils/demo_environment/setup.json +++ b/src/argaze/utils/demo_environment/setup.json @@ -5,7 +5,7 @@ "name": "DICT_APRILTAG_16h5" }, "marker_size": 5, - "camera": "calibration.json", + "optic_parameters": "optic_parameters.json", "parameters": { "cornerRefinementMethod": 1, "aprilTagQuadSigma": 2, diff --git a/src/argaze/utils/demo_heatmap_run.py b/src/argaze/utils/demo_heatmap_run.py index 3e6bb63..df98d33 100644 --- a/src/argaze/utils/demo_heatmap_run.py +++ b/src/argaze/utils/demo_heatmap_run.py @@ -31,7 +31,7 @@ def main(): cv2.imshow(window_name, aoi_frame.heatmap) - # Stop calibration by pressing 'Esc' key + # Stop and save picture by pressing 'Esc' key if cv2.waitKey(10) == 27: current_directory = os.path.dirname(os.path.abspath(__file__)) diff --git a/src/argaze/utils/environment_edit.py b/src/argaze/utils/environment_edit.py index 4fab715..7b6c2fd 100644 --- a/src/argaze/utils/environment_edit.py +++ b/src/argaze/utils/environment_edit.py @@ -164,7 +164,7 @@ def main(): if draw_grid: cv2.putText(video_frame, f'Grid at {z_grid} cm', (500, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) - ar_environment.aruco_detector.camera.draw(video_frame, frame_width/10, frame_height/10, z_grid, color=(127, 127, 127)) + ar_environment.aruco_detector.optic_parameters.draw(video_frame, frame_width/10, frame_height/10, z_grid, color=(127, 127, 127)) # Write timing cv2.putText(video_frame, f'Time: {int(current_frame_time)} ms', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) -- cgit v1.1