diff options
22 files changed, 1375 insertions, 1324 deletions
diff --git a/docs/user_guide/ar_environment/environment_exploitation.md b/docs/user_guide/ar_environment/environment_exploitation.md index 28d61b9..9e4b236 100644 --- a/docs/user_guide/ar_environment/environment_exploitation.md +++ b/docs/user_guide/ar_environment/environment_exploitation.md @@ -1,19 +1,19 @@ Environment exploitation ======================== -Once loaded, [ArEnvironment](../../argaze.md/#argaze.ArFeatures.ArEnvironment) assets can be exploited as illustrated below: +Once loaded, [ArCamera](../../argaze.md/#argaze.ArFeatures.ArCamera) assets can be exploited as illustrated below: ```python # Access to AR environment ArUco detector passing it a image where to detect ArUco markers -ar_environment.aruco_detector.detect_markers(image) +ar_camera.aruco_detector.detect_markers(image) # Access to an AR environment scene -my_first_scene = ar_environment.scenes['my first AR scene'] +my_first_scene = ar_camera.scenes['my first AR scene'] try: # Try to estimate AR scene pose from detected markers - tvec, rmat, consistent_markers = my_first_scene.estimate_pose(ar_environment.aruco_detector.detected_markers) + tvec, rmat, consistent_markers = my_first_scene.estimate_pose(ar_camera.aruco_detector.detected_markers) # Project AR scene into camera image according estimated pose # Optional visual_hfov argument is set to 160° to clip AOI scene according a cone vision diff --git a/docs/user_guide/ar_environment/environment_setup.md b/docs/user_guide/ar_environment/environment_setup.md index f18cc61..1f26d26 100644 --- a/docs/user_guide/ar_environment/environment_setup.md +++ b/docs/user_guide/ar_environment/environment_setup.md @@ -1,9 +1,9 @@ Environment Setup ================= -[ArEnvironment](../../argaze.md/#argaze.ArFeatures.ArEnvironment) setup is loaded from JSON file format. +[ArCamera](../../argaze.md/#argaze.ArFeatures.ArCamera) setup is loaded from JSON file format. -Each [ArEnvironment](../../argaze.md/#argaze.ArFeatures.ArEnvironment) defines a unique [ArUcoDetector](../../argaze.md/#argaze.ArUcoMarkers.ArUcoDetector.ArUcoDetector) dedicated to detection of markers from a specific [ArUcoMarkersDictionary](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersDictionary) and with a given size. However, it is possible to load multiple [ArScene](../../argaze.md/#argaze.ArFeatures.ArScene) into a same [ArEnvironment](../../argaze.md/#argaze.ArFeatures.ArEnvironment). +Each [ArCamera](../../argaze.md/#argaze.ArFeatures.ArCamera) defines a unique [ArUcoDetector](../../argaze.md/#argaze.ArUcoMarkers.ArUcoDetector.ArUcoDetector) dedicated to detection of markers from a specific [ArUcoMarkersDictionary](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersDictionary) and with a given size. However, it is possible to load multiple [ArScene](../../argaze.md/#argaze.ArFeatures.ArScene) into a same [ArCamera](../../argaze.md/#argaze.ArFeatures.ArCamera). Here is JSON environment file example where it is assumed that mentioned .obj files are located relatively to the environment file on disk. @@ -54,13 +54,13 @@ Here is JSON environment file example where it is assumed that mentioned .obj fi }, "scenes": { "my first AR scene" : { - "aruco_scene": "./first_scene/markers.obj", + "aruco_markers_group": "./first_scene/markers.obj", "aoi_scene": "./first_scene/aoi.obj", "angle_tolerance": 15.0, "distance_tolerance": 2.54 }, "my second AR scene" : { - "aruco_scene": "./second_scene/markers.obj", + "aruco_markers_group": "./second_scene/markers.obj", "aoi_scene": "./second_scene/aoi.obj", "angle_tolerance": 15.0, "distance_tolerance": 2.54 @@ -73,5 +73,5 @@ Here is JSON environment file example where it is assumed that mentioned .obj fi from argaze import ArFeatures # Load AR environment -ar_environment = ArFeatures.ArEnvironment.from_json('./environment.json') +ar_camera = ArFeatures.ArCamera.from_json('./environment.json') ``` diff --git a/docs/user_guide/aruco_markers/introduction.md b/docs/user_guide/aruco_markers/introduction.md index dc8d4cb..9d78de0 100644 --- a/docs/user_guide/aruco_markers/introduction.md +++ b/docs/user_guide/aruco_markers/introduction.md @@ -12,4 +12,4 @@ The ArGaze [ArUcoMarkers submodule](../../argaze.md/#argaze.ArUcoMarkers) eases * [ArUcoBoard](../../argaze.md/#argaze.ArUcoMarkers.ArUcoBoard) * [ArUcoOpticCalibrator](../../argaze.md/#argaze.ArUcoMarkers.ArUcoOpticCalibrator) * [ArUcoDetector](../../argaze.md/#argaze.ArUcoMarkers.ArUcoDetector) -* [ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene)
\ No newline at end of file +* [ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup)
\ No newline at end of file diff --git a/docs/user_guide/aruco_markers/markers_scene_description.md b/docs/user_guide/aruco_markers/markers_scene_description.md index e1cd651..c6dbf31 100644 --- a/docs/user_guide/aruco_markers/markers_scene_description.md +++ b/docs/user_guide/aruco_markers/markers_scene_description.md @@ -1,11 +1,11 @@ Markers scene description ========================= -The ArGaze toolkit provides [ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene) class to describe where [ArUcoMarkers](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarker) are placed into a 3D model. +The ArGaze toolkit provides [ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup) class to describe where [ArUcoMarkers](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarker) are placed into a 3D model. - + -[ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene) is useful to: +[ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup) is useful to: * filter markers that belongs to this predefined scene, * check the consistency of detected markers according the place where each marker is expected to be, @@ -37,16 +37,16 @@ f 5//2 6//2 8//2 7//2 ... ``` -Here is a sample of code to show the loading of an [ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene) OBJ file description: +Here is a sample of code to show the loading of an [ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup) OBJ file description: ``` python -from argaze.ArUcoMarkers import ArUcoScene +from argaze.ArUcoMarkers import ArUcoMarkersGroup # Create an ArUco scene from a OBJ file description -aruco_scene = ArUcoScene.ArUcoScene.from_obj('./markers.obj') +aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup.from_obj('./markers.obj') # Print loaded marker places -for place_id, place in aruco_scene.places.items(): +for place_id, place in aruco_markers_group.places.items(): print(f'place {place_id} for marker: ', place.marker.identifier) print(f'place {place_id} translation: ', place.translation) @@ -55,7 +55,7 @@ for place_id, place in aruco_scene.places.items(): ### from JSON -[ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene) description can also be written in a JSON file format. +[ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup) description can also be written in a JSON file format. ``` json { @@ -83,13 +83,13 @@ for place_id, place in aruco_scene.places.items(): Here is a more advanced usage where ArUco scene is built from markers detected into an image: ``` python -from argaze.ArUcoMarkers import ArUcoScene +from argaze.ArUcoMarkers import ArUcoMarkersGroup # Assuming markers have been detected and their pose estimated thanks to ArUcoDetector ... # Build ArUco scene from detected markers -aruco_scene = ArUcoScene.ArUcoScene(aruco_detector.marker_size, aruco_detector.dictionary, aruco_detector.detected_markers) +aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(aruco_detector.marker_size, aruco_detector.dictionary, aruco_detector.detected_markers) ``` ## Markers filtering @@ -97,7 +97,7 @@ aruco_scene = ArUcoScene.ArUcoScene(aruco_detector.marker_size, aruco_detector.d Considering markers are detected, here is how to filter them to consider only those which belongs to the scene: ``` python -scene_markers, remaining_markers = aruco_scene.filter_markers(aruco_detector.detected_markers) +scene_markers, remaining_markers = aruco_markers_group.filter_markers(aruco_detector.detected_markers) ``` ## Marker poses consistency @@ -106,12 +106,12 @@ Then, scene markers poses can be validated by verifying their spatial consistenc ``` python # Check scene markers consistency with 10° angle tolerance and 1 cm distance tolerance -consistent_markers, unconsistent_markers, unconsistencies = aruco_scene.check_markers_consistency(scene_markers, 10, 1) +consistent_markers, unconsistent_markers, unconsistencies = aruco_markers_group.check_markers_consistency(scene_markers, 10, 1) ``` ## Scene pose estimation -Several approaches are available to perform [ArUcoScene](../../argaze.md/#argaze.ArUcoMarkers.ArUcoScene) pose estimation from markers belonging to the scene. +Several approaches are available to perform [ArUcoMarkersGroup](../../argaze.md/#argaze.ArUcoMarkers.ArUcoMarkersGroup) pose estimation from markers belonging to the scene. The first approach considers that scene pose can be estimated **from a single marker pose**: @@ -120,20 +120,20 @@ The first approach considers that scene pose can be estimated **from a single ma marker_id, marker = consistent_markers.popitem() # Estimate scene pose from a single marker -tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker) +tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(marker) ``` The second approach considers that scene pose can be estimated by **averaging several marker poses**: ``` python # Estimate scene pose from all consistent scene markers -tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers) +tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(consistent_markers) ``` The third approach is only available when ArUco markers are placed in such a configuration that is possible to **define orthogonal axis**: ``` python -tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker) +tvec, rmat = self.aruco_markers_group.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker) ``` ## Scene exportation @@ -142,5 +142,5 @@ As ArUco scene can be exported to OBJ file description to import it into most 3D ``` python # Export an ArUco scene as OBJ file description -aruco_scene.to_obj('markers.obj') +aruco_markers_group.to_obj('markers.obj') ``` diff --git a/docs/user_guide/utils/demonstrations_scripts.md b/docs/user_guide/utils/demonstrations_scripts.md index 5d2d760..4f73092 100644 --- a/docs/user_guide/utils/demonstrations_scripts.md +++ b/docs/user_guide/utils/demonstrations_scripts.md @@ -19,7 +19,7 @@ python ./src/argaze/utils/demo_gaze_analysis_run.py ./src/argaze/utils/demo_envi ## Augmented reality pipeline demonstration -Load ArEnvironment from **demo_augmented_reality_setup.json** file then, detect ArUco markers into a demo video source and estimate environment pose. +Load ArCamera from **demo_augmented_reality_setup.json** file then, detect ArUco markers into a demo video source and estimate environment pose. ```shell python ./src/argaze/utils/demo_augmented_reality_run.py ./src/argaze/utils/demo_environment/demo_augmented_reality_setup.json -s ./src/argaze/utils/demo_environment/demo.mov @@ -37,9 +37,9 @@ nav: # - user_guide/areas_of_interest/aoi_matching.md # - user_guide/areas_of_interest/heatmap.md # - Augmented Reality environment: -# - user_guide/ar_environment/introduction.md -# - user_guide/ar_environment/environment_setup.md -# - user_guide/ar_environment/environment_exploitation.md +# - user_guide/ar_camera/introduction.md +# - user_guide/ar_camera/environment_setup.md +# - user_guide/ar_camera/environment_exploitation.md # - Gaze Analysis: # - user_guide/gaze_analysis/introduction.md # - user_guide/gaze_analysis/gaze_position.md diff --git a/src/argaze.test/ArFeatures.py b/src/argaze.test/ArFeatures.py deleted file mode 100644 index 765e9cf..0000000 --- a/src/argaze.test/ArFeatures.py +++ /dev/null @@ -1,75 +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 import ArFeatures - -import numpy - -class TestArEnvironmentClass(unittest.TestCase): - """Test ArEnvironment class.""" - - def test_from_json(self): - """Test ArEnvironment creation from json file.""" - - # Edit test environment file path - current_directory = os.path.dirname(os.path.abspath(__file__)) - json_filepath = os.path.join(current_directory, 'utils/environment.json') - - # Load test environment - ar_environment = ArFeatures.ArEnvironment.from_json(json_filepath) - - # Check environment meta data - self.assertEqual(ar_environment.name, "TestEnvironment") - - # Check ArUco detector - self.assertEqual(ar_environment.aruco_detector.dictionary.name, "DICT_ARUCO_ORIGINAL") - self.assertEqual(ar_environment.aruco_detector.marker_size, 3.0) - self.assertEqual(ar_environment.aruco_detector.parameters.cornerRefinementMethod, 3) - self.assertEqual(ar_environment.aruco_detector.parameters.aprilTagQuadSigma, 2) - self.assertEqual(ar_environment.aruco_detector.parameters.aprilTagDeglitch, 1) - - # 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) - self.assertIsNone(numpy.testing.assert_array_equal(list(ar_environment.scenes.keys()), ["TestSceneA", "TestSceneB"])) - - # Load test scene - ar_scene = ar_environment.scenes["TestSceneA"] - - # Check Aruco scene - self.assertEqual(len(ar_scene.aruco_scene.places), 2) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].translation, [1, 0, 0])) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])) - self.assertEqual(ar_scene.aruco_scene.places[0].marker.identifier, 0) - - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[1].translation, [0, 1, 0])) - self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_scene.places[1].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]])) - self.assertEqual(ar_scene.aruco_scene.places[1].marker.identifier, 1) - - # Check AOI scene - self.assertEqual(len(ar_scene.aoi_scene.items()), 1) - self.assertEqual(ar_scene.aoi_scene['Test'].points_number, 4) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aoi_scene['Test'].size, [1., 1., 0.])) - - # Check ArScene - self.assertEqual(ar_scene.angle_tolerance, 1.0) - self.assertEqual(ar_scene.distance_tolerance, 2.0) - - -if __name__ == '__main__': - - unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarkers/ArUcoCamera.py b/src/argaze.test/ArUcoMarkers/ArUcoCamera.py new file mode 100644 index 0000000..6145f40 --- /dev/null +++ b/src/argaze.test/ArUcoMarkers/ArUcoCamera.py @@ -0,0 +1,74 @@ +#!/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_from_json(self): + """Test ArUcoCamera creation from json file.""" + + # Edit test aruco camera file path + current_directory = os.path.dirname(os.path.abspath(__file__)) + json_filepath = os.path.join(current_directory, 'utils/aruco_camera.json') + + # Load test aruco camera + aruco_camera = ArUcoCamera.ArUcoCamera.from_json(json_filepath) + + # Check aruco camera meta data + self.assertEqual(aruco_camera.name, "TestArUcoCamera") + + # Check ArUco detector + self.assertEqual(aruco_camera.aruco_detector.dictionary.name, "DICT_ARUCO_ORIGINAL") + self.assertEqual(aruco_camera.aruco_detector.marker_size, 3.0) + self.assertEqual(aruco_camera.aruco_detector.parameters.cornerRefinementMethod, 3) + self.assertEqual(aruco_camera.aruco_detector.parameters.aprilTagQuadSigma, 2) + self.assertEqual(aruco_camera.aruco_detector.parameters.aprilTagDeglitch, 1) + + # Check ArUco detector optic parameters + self.assertEqual(aruco_camera.aruco_detector.optic_parameters.rms, 1.0) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.aruco_detector.optic_parameters.dimensions, [1920, 1080])) + self.assertIsNone(numpy.testing.assert_array_equal(aruco_camera.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(aruco_camera.aruco_detector.optic_parameters.D, [-1.0, -0.5, 0.0, 0.5, 1.0])) + + # Check camera scenes + self.assertEqual(len(aruco_camera.scenes), 2) + self.assertIsNone(numpy.testing.assert_array_equal(list(aruco_camera.scenes.keys()), ["TestSceneA", "TestSceneB"])) + + # Load test scene + ar_scene = aruco_camera.scenes["TestSceneA"] + + # Check Aruco scene + self.assertEqual(len(ar_scene.aruco_markers_group.places), 2) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_markers_group.places[0].translation, [1, 0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_markers_group.places[0].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])) + self.assertEqual(ar_scene.aruco_markers_group.places[0].marker.identifier, 0) + + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_markers_group.places[1].translation, [0, 1, 0])) + self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_markers_group.places[1].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]])) + self.assertEqual(ar_scene.aruco_markers_group.places[1].marker.identifier, 1) + + # Check AOI scene + self.assertEqual(len(ar_scene.aoi_scene.items()), 1) + self.assertEqual(ar_scene.aoi_scene['Test'].points_number, 4) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aoi_scene['Test'].size, [1., 1., 0.])) + + # Check ArScene + self.assertEqual(ar_scene.angle_tolerance, 1.0) + self.assertEqual(ar_scene.distance_tolerance, 2.0) + +if __name__ == '__main__': + + unittest.main()
\ No newline at end of file diff --git a/src/argaze.test/ArUcoMarkers/ArUcoScene.py b/src/argaze.test/ArUcoMarkers/ArUcoScene.py index f334542..628eac5 100644 --- a/src/argaze.test/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze.test/ArUcoMarkers/ArUcoScene.py @@ -11,12 +11,12 @@ import unittest import os import math -from argaze.ArUcoMarkers import ArUcoScene, ArUcoMarker +from argaze.ArUcoMarkers import ArUcoMarkersGroup, ArUcoMarker import cv2 as cv import numpy -class TestArUcoSceneClass(unittest.TestCase): +class TestArUcoMarkersGroupClass(unittest.TestCase): def new_from_obj(self): @@ -25,7 +25,7 @@ class TestArUcoSceneClass(unittest.TestCase): obj_filepath = os.path.join(current_directory, 'utils/scene.obj') # Load file - self.aruco_scene = ArUcoScene.ArUcoScene.from_obj(obj_filepath) + self.aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup.from_obj(obj_filepath) def new_from_json(self): @@ -34,7 +34,7 @@ class TestArUcoSceneClass(unittest.TestCase): json_filepath = os.path.join(current_directory, 'utils/scene.json') # Load file - self.aruco_scene = ArUcoScene.ArUcoScene.from_json(json_filepath) + self.aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup.from_json(json_filepath) def setup_markers(self): @@ -47,56 +47,56 @@ class TestArUcoSceneClass(unittest.TestCase): } # Prepare scene markers and remaining markers - self.scene_markers, self.remaining_markers = self.aruco_scene.filter_markers(self.detected_markers) + self.scene_markers, self.remaining_markers = self.aruco_markers_group.filter_markers(self.detected_markers) def test_new_from_obj(self): - """Test ArUcoScene creation.""" + """Test ArUcoMarkersGroup creation.""" self.new_from_obj() self.setup_markers() - # Check ArUcoScene creation - self.assertEqual(len(self.aruco_scene.places), 3) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2])) - self.assertEqual(self.aruco_scene.marker_size, 1.) + # Check ArUcoMarkersGroup creation + self.assertEqual(len(self.aruco_markers_group.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_markers_group.marker_size, 1.) - self.assertEqual(self.aruco_scene.places[0].marker.identifier, 0) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].translation, [0., 0., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) - self.assertEqual(self.aruco_scene.places[1].marker.identifier, 1) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].translation, [10., 10., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) - self.assertEqual(self.aruco_scene.places[2].marker.identifier, 2) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].translation, [0., 10., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) def test_new_from_json(self): - """Test ArUcoScene creation.""" + """Test ArUcoMarkersGroup creation.""" self.new_from_json() self.setup_markers() - # Check ArUcoScene creation - self.assertEqual(len(self.aruco_scene.places), 3) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.identifiers, [0, 1, 2])) - self.assertEqual(self.aruco_scene.marker_size, 1.) + # Check ArUcoMarkersGroup creation + self.assertEqual(len(self.aruco_markers_group.places), 3) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.identifiers, [0, 1, 2])) + self.assertEqual(self.aruco_markers_group.marker_size, 1.) - self.assertEqual(self.aruco_scene.places[0].marker.identifier, 0) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].translation, [0., 0., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[0].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].translation, [0., 0., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[0].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) - self.assertEqual(self.aruco_scene.places[1].marker.identifier, 1) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].translation, [10., 10., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[1].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].translation, [10., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[1].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) - self.assertEqual(self.aruco_scene.places[2].marker.identifier, 2) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].translation, [0., 10., 0.])) - self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_scene.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) + self.assertEqual(self.aruco_markers_group.places[2].marker.identifier, 2) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].translation, [0., 10., 0.])) + self.assertIsNone(numpy.testing.assert_array_equal(self.aruco_markers_group.places[2].rotation, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) def test_filter_markers(self): - """Test ArUcoScene markers filtering.""" + """Test ArUcoMarkersGroup markers filtering.""" self.new_from_obj() self.setup_markers() @@ -105,11 +105,11 @@ class TestArUcoSceneClass(unittest.TestCase): self.assertEqual(len(self.scene_markers), 3) self.assertEqual(len(self.remaining_markers), 1) - self.assertIsNone(numpy.testing.assert_array_equal(list(self.scene_markers.keys()), self.aruco_scene.identifiers)) + self.assertIsNone(numpy.testing.assert_array_equal(list(self.scene_markers.keys()), self.aruco_markers_group.identifiers)) self.assertIsNone(numpy.testing.assert_array_equal(list(self.remaining_markers.keys()), [3])) def test_check_markers_consistency(self): - """Test ArUcoScene markers consistency checking.""" + """Test ArUcoMarkersGroup markers consistency checking.""" self.new_from_obj() self.setup_markers() @@ -125,7 +125,7 @@ class TestArUcoSceneClass(unittest.TestCase): self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # Check consistency - consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(self.scene_markers, 1, 1) + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(self.scene_markers, 1, 1) # Check consistent markers, unconsistent markers and unconsistencies self.assertEqual(len(consistent_markers), 3) @@ -133,13 +133,13 @@ class TestArUcoSceneClass(unittest.TestCase): self.assertEqual(len(unconsistencies['rotation']), 0) self.assertEqual(len(unconsistencies['translation']), 0) - self.assertIsNone(numpy.testing.assert_array_equal(list(consistent_markers.keys()), self.aruco_scene.identifiers)) + self.assertIsNone(numpy.testing.assert_array_equal(list(consistent_markers.keys()), self.aruco_markers_group.identifiers)) # Edit unconsistent marker poses self.scene_markers[2].translation = numpy.array([5., 15., 5.]) # Check consistency - consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(self.scene_markers, 1, 1) + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(self.scene_markers, 1, 1) # Check consistent markers, unconsistent markers and unconsistencies self.assertEqual(len(consistent_markers), 2) @@ -153,7 +153,7 @@ class TestArUcoSceneClass(unittest.TestCase): self.assertIsNone(numpy.testing.assert_array_equal(list(unconsistencies['translation']['1/2'].keys()), ['current', 'expected'])) def test_estimate_pose_from_single_marker(self): - """Test ArUcoScene pose estimation from single marker.""" + """Test ArUcoMarkersGroup pose estimation from single marker.""" self.new_from_obj() self.setup_markers() @@ -163,13 +163,13 @@ class TestArUcoSceneClass(unittest.TestCase): self.scene_markers[0].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # Estimate pose - tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(self.scene_markers[0]) + tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(self.scene_markers[0]) self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) def test_estimate_pose_from_markers(self): - """Test ArUcoScene pose estimation from markers.""" + """Test ArUcoMarkersGroup pose estimation from markers.""" self.new_from_obj() self.setup_markers() @@ -185,14 +185,14 @@ class TestArUcoSceneClass(unittest.TestCase): self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # Estimate pose - tvec, rmat = self.aruco_scene.estimate_pose_from_markers(self.scene_markers) + tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(self.scene_markers) self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])) - @unittest.skip("ArUcoScene estimate_pose_from_axis_markers method is broken.") + @unittest.skip("ArUcoMarkersGroup estimate_pose_from_axis_markers method is broken.") def test_estimate_pose_from_axis_markers(self): - """Test ArUcoScene pose estimation from axis markers.""" + """Test ArUcoMarkersGroup pose estimation from axis markers.""" self.new_from_obj() self.setup_markers() @@ -208,7 +208,7 @@ class TestArUcoSceneClass(unittest.TestCase): self.scene_markers[2].rotation = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # Estimate pose - tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(self.scene_markers[2], self.scene_markers[1], self.scene_markers[0]) + tvec, rmat = self.aruco_markers_group.estimate_pose_from_axis_markers(self.scene_markers[2], self.scene_markers[1], self.scene_markers[0]) self.assertIsNone(numpy.testing.assert_array_equal(tvec, [1., 1., 5.])) self.assertIsNone(numpy.testing.assert_array_equal(rmat, [[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])) diff --git a/src/argaze.test/utils/environment.json b/src/argaze.test/ArUcoMarkers/utils/aruco_camera.json index df1c771..7648916 100644 --- a/src/argaze.test/utils/environment.json +++ b/src/argaze.test/ArUcoMarkers/utils/aruco_camera.json @@ -1,5 +1,5 @@ { - "name": "TestEnvironment", + "name": "TestArUcoCamera", "aruco_detector": { "dictionary": { "name": "DICT_ARUCO_ORIGINAL" @@ -44,7 +44,7 @@ }, "scenes": { "TestSceneA" : { - "aruco_scene": { + "aruco_markers_group": { "marker_size": 3.0, "dictionary": { "name": "DICT_ARUCO_ORIGINAL" @@ -65,7 +65,7 @@ "distance_tolerance": 2.0 }, "TestSceneB" : { - "aruco_scene": { + "aruco_markers_group": { "marker_size": 3.0, "dictionary": { "name": "DICT_ARUCO_ORIGINAL" diff --git a/src/argaze.test/ArUcoMarkers/utils/scene.obj b/src/argaze.test/ArUcoMarkers/utils/scene.obj index 16c22a0..c233da2 100644 --- a/src/argaze.test/ArUcoMarkers/utils/scene.obj +++ b/src/argaze.test/ArUcoMarkers/utils/scene.obj @@ -1,4 +1,4 @@ -# .OBJ file for ArUcoScene unitary test +# .OBJ file for ArUcoMarkersGroup unitary test o DICT_ARUCO_ORIGINAL#0_Marker v -0.500000 -0.500000 0.000000 v 0.500000 -0.500000 0.000000 diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py index f68fe12..b7ac48c 100644 --- a/src/argaze/ArFeatures.py +++ b/src/argaze/ArFeatures.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -"""Manage AR environement assets.""" +"""ArGaze pipeline assets.""" __author__ = "Théo de la Hogue" __credits__ = [] @@ -17,7 +17,6 @@ import threading import time from argaze import DataStructures, GazeFeatures -from argaze.ArUcoMarkers import * from argaze.AreaOfInterest import * from argaze.GazeAnalysis import * @@ -33,7 +32,7 @@ ArFrameType = TypeVar('ArFrame', bound="ArFrame") ArSceneType = TypeVar('ArScene', bound="ArScene") # Type definition for type annotation convenience -ArEnvironmentType = TypeVar('ArEnvironment', bound="ArEnvironment") +ArCameraType = TypeVar('ArCamera', bound="ArCamera") # Type definition for type annotation convenience class PoseEstimationFailed(Exception): @@ -49,7 +48,7 @@ class PoseEstimationFailed(Exception): class SceneProjectionFailed(Exception): """ - Exception raised by ArEnvironment detect_and_project method when the scene can't be projected. + Exception raised by ArCamera detect_and_project method when the scene can't be projected. """ def __init__(self, message): @@ -567,7 +566,7 @@ class ArFrame(): scan_path: GazeFeatures.ScanPath = field(default_factory=GazeFeatures.ScanPath) scan_path_analyzers: dict = field(default_factory=dict) heatmap: AOIFeatures.Heatmap = field(default_factory=AOIFeatures.Heatmap) - background: numpy.array = field(default_factory=numpy.array) + background: numpy.array = field(default_factory=lambda : numpy.array([])) layers: dict = field(default_factory=dict) log: bool = field(default=False) image_parameters: dict = field(default_factory=DEFAULT_ARFRAME_IMAGE_PARAMETERS) @@ -1016,11 +1015,19 @@ class ArFrame(): draw_gaze_position: [GazeFeatures.GazePosition.draw](argaze.md/#argaze.GazeFeatures.GazePosition.draw) parameters (if None, no gaze position is drawn) """ + print('type', type(self)) + + print('ArFrame.image 1') + # Use image_parameters attribute if no parameters if background_weight is None and heatmap_weight is None and draw_scan_path is None and draw_layers is None and draw_gaze_position is None: + print('ArFrame.image 2') + return self.image(**self.image_parameters) + print('ArFrame.image 3') + # Lock frame exploitation self.__look_lock.acquire() @@ -1066,38 +1073,30 @@ class ArFrame(): # Unlock frame exploitation self.__look_lock.release() + print('ArFrame.image', image.shape) + return image @dataclass class ArScene(): """ - Define an Augmented Reality scene with ArUcoMarkers, ArLayers and ArFrames inside. + Define abstract Augmented Reality scene with ArLayers and ArFrames inside. Parameters: name: name of the scene - aruco_scene: ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below. - layers: dictionary of ArLayers to project once the pose is estimated: see [project][argaze.ArFeatures.ArScene.project] function below. frames: dictionary to ArFrames to project once the pose is estimated: see [project][argaze.ArFeatures.ArScene.project] function below. - aruco_axis: Optional dictionary to define orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin). \ - This pose estimation strategy is used by [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function when at least 3 markers are detected. - - aruco_aoi: Optional dictionary of AOI defined by list of markers identifier and markers corners index tuples: see [build_aruco_aoi_scene][argaze.ArFeatures.ArScene.build_aruco_aoi_scene] function below. - angle_tolerance: Optional angle error tolerance to validate marker pose in degree used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function. distance_tolerance: Optional distance error tolerance to validate marker pose in centimeter used into [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function. """ name: str - aruco_scene: ArUcoScene.ArUcoScene = field(default_factory=ArUcoScene.ArUcoScene) layers: dict = field(default_factory=dict) frames: dict = field(default_factory=dict) - aruco_axis: dict = field(default_factory=dict) - aruco_aoi: dict = field(default_factory=dict) angle_tolerance: float = field(default=0.) distance_tolerance: float = field(default=0.) @@ -1130,7 +1129,6 @@ class ArScene(): """ output = f'parent:\n{self.parent.name}\n' - output += f'ArUcoScene:\n{self.aruco_scene}\n' if len(self.layers): output += f'ArLayers:\n' @@ -1157,8 +1155,15 @@ class ArScene(): self.__parent = parent @classmethod - def from_dict(self, scene_data, working_directory: str = None) -> ArSceneType: + def from_dict(self, scene_data: dict, working_directory: str = None) -> ArSceneType: + """ + Load ArScene from dictionary. + Parameters: + scene_data: dictionary + working_directory: folder path where to load files when a dictionary value is a relative filepath. + """ + # Load name try: @@ -1168,27 +1173,6 @@ class ArScene(): new_scene_name = None - # Load aruco scene - try: - - # Check aruco_scene value type - aruco_scene_value = scene_data.pop('aruco_scene') - - # str: relative path to .obj file - if type(aruco_scene_value) == str: - - aruco_scene_value = os.path.join(working_directory, aruco_scene_value) - new_aruco_scene = ArUcoScene.ArUcoScene.from_obj(aruco_scene_value) - - # dict: - else: - - new_aruco_scene = ArUcoScene.ArUcoScene(**aruco_scene_value) - - except KeyError: - - new_aruco_scene = None - # Load layers new_layers = {} @@ -1272,70 +1256,20 @@ class ArScene(): pass - return ArScene(new_scene_name, new_aruco_scene, new_layers, new_frames, **scene_data) + return ArScene(new_scene_name, new_layers, new_frames, **scene_data) - def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]: - """Estimate scene pose from detected ArUco markers. + def estimate_pose(self, detected_features) -> Tuple[numpy.array, numpy.array]: + """Define abstract estimate scene pose method. + + Parameters: + detected_features: any features detected by parent ArCamera that will help in scene pose estimation. Returns: - scene translation vector - scene rotation matrix - pose estimation strategy - dict of markers used to estimate the pose + tvec: scene translation vector + rvec: scene rotation matrix """ - # Pose estimation fails when no marker is detected - if len(detected_markers) == 0: - - raise PoseEstimationFailed('No marker detected') - - scene_markers, _ = self.aruco_scene.filter_markers(detected_markers) - - # Pose estimation fails when no marker belongs to the scene - if len(scene_markers) == 0: - - raise PoseEstimationFailed('No marker belongs to the scene') - - # Estimate scene pose from unique marker transformations - elif len(scene_markers) == 1: - - marker_id, marker = scene_markers.popitem() - tvec, rmat = self.aruco_scene.estimate_pose_from_single_marker(marker) - - return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker} - - # Try to estimate scene pose from 3 markers defining an orthogonal axis - elif len(scene_markers) >= 3 and len(self.aruco_axis) > 0: - - for axis_name, axis_markers in self.aruco_axis.items(): - - try: - - origin_marker = scene_markers[axis_markers['origin_marker']] - horizontal_axis_marker = scene_markers[axis_markers['horizontal_axis_marker']] - vertical_axis_marker = scene_markers[axis_markers['vertical_axis_marker']] - - tvec, rmat = self.aruco_scene.estimate_pose_from_axis_markers(origin_marker, horizontal_axis_marker, vertical_axis_marker) - - return tvec, rmat, 'estimate_pose_from_axis_markers', {origin_marker.identifier: origin_marker, horizontal_axis_marker.identifier: horizontal_axis_marker, vertical_axis_marker.identifier: vertical_axis_marker} - - except: - pass - - raise PoseEstimationFailed('No marker axis') - - # Otherwise, check markers consistency - consistent_markers, unconsistent_markers, unconsistencies = self.aruco_scene.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance) - - # Pose estimation fails when no marker passes consistency checking - if len(consistent_markers) == 0: - - raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies) - - # Otherwise, estimate scene pose from all consistent markers pose - tvec, rmat = self.aruco_scene.estimate_pose_from_markers(consistent_markers) - - return tvec, rmat, 'estimate_pose_from_markers', consistent_markers + raise NotImplementedError('estimate_pose() method not implemented') def project(self, tvec: numpy.array, rvec: numpy.array, visual_hfov: float = 0.) -> Tuple[str, AOI2DScene.AOI2DScene]: """Project layers according estimated pose and optional horizontal field of view clipping angle. @@ -1374,52 +1308,6 @@ class ArScene(): # Project layer aoi scene yield name, aoi_scene_copy.project(tvec, rvec, self.parent.aruco_detector.optic_parameters.K) - def build_aruco_aoi_scene(self, detected_markers) -> AOI2DScene.AOI2DScene: - """ - Build AOI scene from detected ArUco markers as defined in aruco_aoi dictionary. - - Returns: - aoi_2d_scene: built AOI 2D scene - """ - - # ArUco aoi must be defined - assert(self.aruco_aoi) - - # AOI projection fails when no marker is detected - if len(detected_markers) == 0: - - raise SceneProjectionFailed('No marker detected') - - aruco_aoi_scene = {} - - for aruco_aoi_name, aoi in self.aruco_aoi.items(): - - # Each aoi's corner is defined by a marker's corner - aoi_corners = [] - for corner in ["upper_left_corner", "upper_right_corner", "lower_right_corner", "lower_left_corner"]: - - marker_identifier = aoi[corner]["marker_identifier"] - - try: - - aoi_corners.append(detected_markers[marker_identifier].corners[0][aoi[corner]["marker_corner_index"]]) - - except Exception as e: - - raise SceneProjectionFailed(f'Missing marker #{e} to build ArUco AOI scene') - - aruco_aoi_scene[aruco_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners) - - # Then each inner aoi is projected from the current aruco aoi - for inner_aoi_name, inner_aoi in self.aoi_3d_scene.items(): - - if aruco_aoi_name != inner_aoi_name: - - aoi_corners = [numpy.array(aruco_aoi_scene[aruco_aoi_name].outter_axis(inner)) for inner in self.__orthogonal_projection_cache[inner_aoi_name]] - aruco_aoi_scene[inner_aoi_name] = AOIFeatures.AreaOfInterest(aoi_corners) - - return AOI2DScene.AOI2DScene(aruco_aoi_scene) - def draw_axis(self, image: numpy.array): """ Draw scene axis into image. @@ -1428,212 +1316,95 @@ class ArScene(): image: where to draw """ - self.aruco_scene.draw_axis(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D) - - def draw_places(self, image: numpy.array): - """ - Draw scene places into image. - - Parameters: - image: where to draw - """ - - self.aruco_scene.draw_places(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D) - -# Define default ArEnvironment image_paremeters values -DEFAULT_ARENVIRONMENT_IMAGE_PARAMETERS = { - "draw_detected_markers": { - "color": (0, 255, 0), - "draw_axes": { - "thickness": 3 - } - } -} + raise NotImplementedError('draw_axis() method not implemented') @dataclass -class ArEnvironment(): +class ArCamera(ArFrame): """ - Define Augmented Reality environment based on ArUco marker detection. + Define abstract Augmented Reality camera as ArFrame with ArScenes inside. Parameters: - name: environment name - aruco_detector: ArUco marker detector - camera_frame: where to project scenes - scenes: all environment scenes + scenes: all scenes to project into camera frame """ - name: str - aruco_detector: ArUcoDetector.ArUcoDetector = field(default_factory=ArUcoDetector.ArUcoDetector) - camera_frame: ArFrame = field(default_factory=ArFrame) scenes: dict = field(default_factory=dict) - image_parameters: dict = field(default_factory=DEFAULT_ARENVIRONMENT_IMAGE_PARAMETERS) def __post_init__(self): - # Setup camera frame parent attribute - if self.camera_frame is not None: - - self.camera_frame.parent = self + # Init ArFrame + super().__post_init__() # Setup scenes parent attribute for name, scene in self.scenes.items(): scene.parent = self - # Init a lock to share AOI scene projections into camera frame between multiple threads - self.__camera_frame_lock = threading.Lock() - - # Define public timestamp buffer to store ignored gaze positions - self.ignored_gaze_positions = GazeFeatures.TimeStampedGazePositions() - - @classmethod - def from_dict(self, environment_data, working_directory: str = None) -> ArEnvironmentType: - - new_environment_name = environment_data.pop('name') - - try: - new_detector_data = environment_data.pop('aruco_detector') - - new_aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(**new_detector_data.pop('dictionary')) - new_marker_size = new_detector_data.pop('marker_size') - - # Check optic_parameters value type - optic_parameters_value = new_detector_data.pop('optic_parameters') - - # str: relative path to .json file - if type(optic_parameters_value) == str: - - optic_parameters_value = os.path.join(working_directory, optic_parameters_value) - new_optic_parameters = ArUcoOpticCalibrator.OpticParameters.from_json(optic_parameters_value) - - # dict: - else: - - new_optic_parameters = ArUcoOpticCalibrator.OpticParameters(**optic_parameters_value) - - # Check detector parameters value type - detector_parameters_value = new_detector_data.pop('parameters') - - # str: relative path to .json file - if type(detector_parameters_value) == str: - - detector_parameters_value = os.path.join(working_directory, detector_parameters_value) - new_aruco_detector_parameters = ArUcoDetector.DetectorParameters.from_json(detector_parameters_value) - - # dict: - else: - - new_aruco_detector_parameters = ArUcoDetector.DetectorParameters(**detector_parameters_value) - - new_aruco_detector = ArUcoDetector.ArUcoDetector(new_aruco_dictionary, new_marker_size, new_optic_parameters, new_aruco_detector_parameters) - - except KeyError: - - new_aruco_detector = None - - # Load camera frame as large as aruco dectector optic parameters - try: - - camera_frame_data = environment_data.pop('camera_frame') - - # Create camera frame - new_camera_frame = ArFrame.from_dict(camera_frame_data, working_directory) - - # Setup camera frame - new_camera_frame.name = new_environment_name - new_camera_frame.size = new_optic_parameters.dimensions - new_camera_frame.background = numpy.zeros((new_optic_parameters.dimensions[1], new_optic_parameters.dimensions[0], 3)).astype(numpy.uint8) - - except KeyError: - - new_camera_frame = None - - # Build scenes - new_scenes = {} - for scene_name, scene_data in environment_data.pop('scenes').items(): - - # Append name - scene_data['name'] = scene_name - - # Create new scene - new_scene = ArScene.from_dict(scene_data, working_directory) - - # Append new scene - new_scenes[scene_name] = new_scene - - # Setup expected aoi of each camera frame layer aoi scan path with the aoi of corresponding scene layer - if new_camera_frame is not None: - - for camera_frame_layer_name, camera_frame_layer in new_camera_frame.layers.items(): + # Setup expected aoi of each layer aoi scan path with the aoi of corresponding scene layer + for layer_name, layer in self.layers.items(): - if camera_frame_layer.aoi_scan_path is not None: + if layer.aoi_scan_path is not None: - all_aoi_list = [] + all_aoi_list = [] - for scene_name, scene in new_scenes.items(): + for scene_name, scene in new_scenes.items(): - try: + try: - scene_layer = scene.layers[camera_frame_layer_name] + scene_layer = scene.layers[layer_name] - all_aoi_list.extend(list(scene_layer.aoi_scene.keys())) + all_aoi_list.extend(list(scene_layer.aoi_scene.keys())) - except KeyError: + except KeyError: - continue + continue - camera_frame_layer.aoi_scan_path.expected_aois = all_aoi_list + layer.aoi_scan_path.expected_aois = all_aoi_list - # Load environment image parameters - try: + # Init a lock to share scene projections into camera frame between multiple threads + self._frame_lock = threading.Lock() - new_environment_image_parameters = environment_data.pop('image_parameters') + # Define public timestamp buffer to store ignored gaze positions + self.ignored_gaze_positions = GazeFeatures.TimeStampedGazePositions() + + def __str__(self) -> str: + """ + Returns: + String representation + """ - except KeyError: + output = f'Name:\n{self.name}\n' - new_environment_image_parameters = DEFAULT_ARENVIRONMENT_IMAGE_PARAMETERS + for name, scene in self.scenes.items(): + output += f'\"{name}\" ArScene:\n{scene}\n' - # Create new environment - return ArEnvironment(new_environment_name, \ - new_aruco_detector, \ - new_camera_frame, \ - new_scenes, \ - new_environment_image_parameters \ - ) + return output @classmethod - def from_json(self, json_filepath: str) -> ArEnvironmentType: + def from_dict(self, camera_data: dict, working_directory: str = None) -> ArCameraType: """ - Load ArEnvironment from .json file. + Load ArCamera from dictionary. Parameters: - json_filepath: path to json file + camera_data: dictionary + working_directory: folder path where to load files when a dictionary value is a relative filepath. """ - with open(json_filepath) as configuration_file: - - environment_data = json.load(configuration_file) - working_directory = os.path.dirname(json_filepath) - - return ArEnvironment.from_dict(environment_data, working_directory) + raise NotImplementedError('from_dict() method not implemented') - def __str__(self) -> str: - """ - Returns: - String representation + @classmethod + def from_json(self, json_filepath: str) -> ArCameraType: """ + Load ArCamera from .json file. - output = f'Name:\n{self.name}\n' - output += f'ArUcoDetector:\n{self.aruco_detector}\n' - - for name, scene in self.scenes.items(): - output += f'\"{name}\" ArScene:\n{scene}\n' + Parameters: + json_filepath: path to json file + """ - return output + raise NotImplementedError('from_json() method not implemented') @property def frames(self): - """Iterate over all environment scenes frames""" + """Iterate over all camera scenes frames""" # For each scene for scene_name, scene in self.scenes.items(): @@ -1644,84 +1415,18 @@ class ArEnvironment(): yield frame def detect_and_project(self, image: numpy.array) -> Tuple[float, dict]: - """Detect environment aruco markers from image and project scenes into camera frame. - - Returns: - - detection_time: aruco marker detection time in ms - - exceptions: dictionary with exception raised per scene - """ - - # Detect aruco markers - detection_time = self.aruco_detector.detect_markers(image) - - # Lock camera frame exploitation - self.__camera_frame_lock.acquire() - - # Fill camera frame background with image - self.camera_frame.background = image - - # Clear former layers projection into camera frame - for came_layer_name, camera_layer in self.camera_frame.layers.items(): - - camera_layer.aoi_scene = AOI2DScene.AOI2DScene() - - # Store exceptions for each scene - exceptions = {} - - # Project each aoi 3d scene into camera frame - for scene_name, scene in self.scenes.items(): - - ''' TODO: Enable aruco_aoi processing - if scene.aruco_aoi: - - try: - - # Build AOI scene directly from detected ArUco marker corners - self.camera_frame.aoi_2d_scene |= scene.build_aruco_aoi_scene(self.aruco_detector.detected_markers) - - except SceneProjectionFailed: - - pass - ''' - - try: - - # Estimate scene markers poses - self.aruco_detector.estimate_markers_pose(scene.aruco_scene.identifiers) - - # Estimate scene pose from detected scene markers - tvec, rmat, _, _ = scene.estimate_pose(self.aruco_detector.detected_markers) - - # Project scene into camera frame according estimated pose - for layer_name, layer_projection in scene.project(tvec, rmat): - - try: + """Detect AR features from image and project scenes into camera frame.""" - self.camera_frame.layers[layer_name].aoi_scene |= layer_projection - - except KeyError: - - pass - - # Store exceptions and continue - except Exception as e: - - exceptions[scene_name] = e - - # Unlock camera frame exploitation - self.__camera_frame_lock.release() - - # Return dection time and exceptions - return detection_time, exceptions + raise NotImplementedError('detect_and_project() method not implemented') def look(self, timestamp: int|float, gaze_position: GazeFeatures.GazePosition): - """Project timestamped gaze position into each frame. + """Project timestamped gaze position into each scene frames. !!! warning detect_and_project method needs to be called first. """ # Can't use camera frame when it is locked - if self.__camera_frame_lock.locked(): + if self._frame_lock.locked(): # TODO: Store ignored timestamped gaze positions for further projections # PB: This would imply to also store frame projections !!! @@ -1730,12 +1435,12 @@ class ArEnvironment(): return # Lock camera frame exploitation - self.__camera_frame_lock.acquire() + self._frame_lock.acquire() # Project gaze position into camera frame - yield self.camera_frame, self.camera_frame.look(timestamp, gaze_position) + yield self, self.look(timestamp, gaze_position) - # Project gaze position into each frame if possible + # Project gaze position into each scene frames if possible for frame in self.frames: # Is there an AOI inside camera frame layers projection which its name equals to a frame name? @@ -1761,7 +1466,7 @@ class ArEnvironment(): pass # Unlock camera frame exploitation - self.__camera_frame_lock.release() + self._frame_lock.release() def map(self): """Project camera frame background into frames background. @@ -1770,11 +1475,11 @@ class ArEnvironment(): """ # Can't use camera frame when it is locked - if self.__camera_frame_lock.locked(): + if self._frame_lock.locked(): return # Lock camera frame exploitation - self.__camera_frame_lock.acquire() + self._frame_lock.acquire() # Project image into each frame if possible for frame in self.frames: @@ -1798,43 +1503,19 @@ class ArEnvironment(): pass # Unlock camera frame exploitation - self.__camera_frame_lock.release() - - def image(self, draw_detected_markers: dict = None): - """Get camera frame projections with ArUco detection visualisation. + self._frame_lock.release() - Parameters: - image: image where to draw - draw_detected_markers: ArucoMarker.draw parameters (if None, no marker drawn) + def image(self, **frame_image_parameters) -> numpy.array: + """ + Get camera frame image. """ - # Use image_parameters attribute if no parameters - if draw_detected_markers is None: - - return self.image(**self.image_parameters) - - # Can't use camera frame when it is locked - if self.__camera_frame_lock.locked(): - return - - # Lock camera frame exploitation - self.__camera_frame_lock.acquire() - - # Get camera frame image - image = self.camera_frame.image() - - # Draw detected markers if required - if draw_detected_markers is not None: - - self.aruco_detector.draw_detected_markers(image, draw_detected_markers) - - # Unlock camera frame exploitation - self.__camera_frame_lock.release() + print('ArCamera.image') - return image + return super().image(**frame_image_parameters) def to_json(self, json_filepath): - """Save environment to .json file.""" + """Save camera to .json file.""" with open(json_filepath, 'w', encoding='utf-8') as file: diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py new file mode 100644 index 0000000..453c18b --- /dev/null +++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py @@ -0,0 +1,264 @@ +#!/usr/bin/env python + +"""ArCamera based of ArUco markers technology.""" + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "BSD" + +from typing import TypeVar, Tuple +from dataclasses import dataclass, field +import json +import os + +from argaze import ArFeatures, DataStructures +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoDetector, ArUcoOpticCalibrator, ArUcoScene +from argaze.AreaOfInterest import AOI2DScene + +import cv2 +import numpy + +ArUcoCameraType = TypeVar('ArUcoCamera', bound="ArUcoCamera") +# Type definition for type annotation convenience + +# Define default ArUcoCamera image_paremeters values +DEFAULT_ARUCOCAMERA_IMAGE_PARAMETERS = { + "draw_detected_markers": { + "color": (0, 255, 0), + "draw_axes": { + "thickness": 3 + } + } +} + +@dataclass +class ArUcoCamera(ArFeatures.ArCamera): + """ + Define an ArCamera based on ArUco marker detection. + + aruco_detector: ArUco marker detector + """ + + aruco_detector: ArUcoDetector.ArUcoDetector = field(default_factory=ArUcoDetector.ArUcoDetector) + + def __post_init__(self): + + super().__post_init__() + + # Camera frame size should be equals to optic parameters dimensions + assert(self.size == self.aruco_detector.optic_parameters.dimensions) + + def __str__(self) -> str: + """ + Returns: + String representation + """ + + output = super().__str__() + output += f'ArUcoDetector:\n{self.aruco_detector}\n' + + return output + + @classmethod + def from_dict(self, aruco_camera_data, working_directory: str = None) -> ArUcoCameraType: + """ + Load ArUcoCamera from dictionary. + + Parameters: + aruco_camera_data: dictionary + working_directory: folder path where to load files when a dictionary value is a relative filepath. + """ + + # Load ArUco detector + try: + new_detector_data = aruco_camera_data.pop('aruco_detector') + + new_aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(**new_detector_data.pop('dictionary')) + new_marker_size = new_detector_data.pop('marker_size') + + # Check optic_parameters value type + optic_parameters_value = new_detector_data.pop('optic_parameters') + + # str: relative path to .json file + if type(optic_parameters_value) == str: + + optic_parameters_value = os.path.join(working_directory, optic_parameters_value) + new_optic_parameters = ArUcoOpticCalibrator.OpticParameters.from_json(optic_parameters_value) + + # dict: + else: + + new_optic_parameters = ArUcoOpticCalibrator.OpticParameters(**optic_parameters_value) + + # Check detector parameters value type + detector_parameters_value = new_detector_data.pop('parameters') + + # str: relative path to .json file + if type(detector_parameters_value) == str: + + detector_parameters_value = os.path.join(working_directory, detector_parameters_value) + new_aruco_detector_parameters = ArUcoDetector.DetectorParameters.from_json(detector_parameters_value) + + # dict: + else: + + new_aruco_detector_parameters = ArUcoDetector.DetectorParameters(**detector_parameters_value) + + new_aruco_detector = ArUcoDetector.ArUcoDetector(new_aruco_dictionary, new_marker_size, new_optic_parameters, new_aruco_detector_parameters) + + except KeyError: + + new_aruco_detector = None + + # Load ArUcoScenes + new_scenes = {} + for aruco_scene_name, aruco_scene_data in aruco_camera_data.pop('scenes').items(): + + # Append name + aruco_scene_data['name'] = aruco_scene_name + + # Create new aruco scene + new_aruco_scene = ArUcoScene.ArUcoScene.from_dict(aruco_scene_data, working_directory) + + # Append new scene + new_scenes[aruco_scene_name] = new_aruco_scene + + # Load image parameters + try: + + new_image_parameters = aruco_camera_data.pop('image_parameters') + + except KeyError: + + new_image_parameters = {**DEFAULT_ARFRAME_IMAGE_PARAMETERS, **DEFAULT_ARUCOCAMERA_IMAGE_PARAMETERS} + + # Get values of temporary ar frame created from aruco_camera_data + temp_ar_frame_values = DataStructures.as_dict(ArFeatures.ArFrame.from_dict(aruco_camera_data, working_directory)) + + # Remove values from temporary ar frame scenes + temp_ar_frame_values.pop('image_parameters') + + # Create new aruco camera using temporary ar frame values + return ArUcoCamera(aruco_detector=new_aruco_detector, scenes=new_scenes, image_parameters=new_image_parameters, **temp_ar_frame_values) + + @classmethod + def from_json(self, json_filepath: str) -> ArUcoCameraType: + """ + Load ArUcoCamera from .json file. + + Parameters: + json_filepath: path to json file + """ + + with open(json_filepath) as configuration_file: + + aruco_camera_data = json.load(configuration_file) + working_directory = os.path.dirname(json_filepath) + + return ArUcoCamera.from_dict(aruco_camera_data, working_directory) + + def detect_and_project(self, image: numpy.array) -> Tuple[float, dict]: + """Detect environment aruco markers from image and project scenes into camera frame. + + Returns: + - detection_time: aruco marker detection time in ms + - exceptions: dictionary with exception raised per scene + """ + + # Detect aruco markers + detection_time = self.aruco_detector.detect_markers(image) + + # Lock camera frame exploitation + self._frame_lock.acquire() + + # Fill camera frame background with image + self.background = image + + # Clear former layers projection into camera frame + for layer_name, layer in self.layers.items(): + + layer.aoi_scene = AOI2DScene.AOI2DScene() + + # Store exceptions for each scene + exceptions = {} + + # Project each aoi 3d scene into camera frame + for scene_name, scene in self.scenes.items(): + + ''' TODO: Enable aruco_aoi processing + if scene.aruco_aoi: + + try: + + # Build AOI scene directly from detected ArUco marker corners + self.camera_frame.aoi_2d_scene |= scene.build_aruco_aoi_scene(self.aruco_detector.detected_markers) + + except SceneProjectionFailed: + + pass + ''' + + try: + + # Estimate scene markers poses + self.aruco_detector.estimate_markers_pose(scene.aruco_markers_group.identifiers) + + # Estimate scene pose from detected scene markers + tvec, rmat, _, _ = scene.estimate_pose(self.aruco_detector.detected_markers) + + # Project scene into camera frame according estimated pose + for layer_name, layer_projection in scene.project(tvec, rmat): + + try: + + self.camera_frame.layers[layer_name].aoi_scene |= layer_projection + + except KeyError: + + pass + + # Store exceptions and continue + except Exception as e: + + exceptions[scene_name] = e + + # Unlock camera frame exploitation + self._frame_lock.release() + + # Return dection time and exceptions + return detection_time, exceptions + + def image(self, draw_detected_markers: dict = None, **frame_image_parameters): + """Get camera frame projections with ArUco detection visualisation. + + Parameters: + draw_detected_markers: ArucoMarker.draw parameters (if None, no marker drawn) + draw_frame: draw ArFrame image + """ + + print('ArUcoCamera.image 1') + + # Can't use camera frame when it is locked + if self._frame_lock.locked(): + return + + # Lock camera frame exploitation + self._frame_lock.acquire() + + print('ArUcoCamera.image 2') + + # Get camera frame image + image = super().image(**frame_image_parameters) + + print('ArUcoCamera.image 3') + + # Draw detected markers if required + if draw_detected_markers is not None: + + self.aruco_detector.draw_detected_markers(image, draw_detected_markers) + + # Unlock camera frame exploitation + self._frame_lock.release() + + return image diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py new file mode 100644 index 0000000..bdcf70c --- /dev/null +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py @@ -0,0 +1,717 @@ +#!/usr/bin/env python + +""" """ + +__author__ = "Théo de la Hogue" +__credits__ = [] +__copyright__ = "Copyright 2023, Ecole Nationale de l'Aviation Civile (ENAC)" +__license__ = "BSD" + +from typing import TypeVar, Tuple +from dataclasses import dataclass, field +import json +import math +import itertools +import re + +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator + +import numpy +import cv2 as cv +import cv2.aruco as aruco + +T0 = numpy.array([0., 0., 0.]) +"""Define no translation vector.""" + +R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) +"""Define no rotation matrix.""" + +ArUcoMarkersGroupType = TypeVar('ArUcoMarkersGroup', bound="ArUcoMarkersGroup") +# Type definition for type annotation convenience + +def make_rotation_matrix(x, y, z): + + # Create rotation matrix around x axis + c = numpy.cos(numpy.deg2rad(x)) + s = numpy.sin(numpy.deg2rad(x)) + Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + + # Create rotation matrix around y axis + c = numpy.cos(numpy.deg2rad(y)) + s = numpy.sin(numpy.deg2rad(y)) + Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) + + # Create rotation matrix around z axis + c = numpy.cos(numpy.deg2rad(z)) + s = numpy.sin(numpy.deg2rad(z)) + Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + # Return intrinsic rotation matrix + return Rx.dot(Ry.dot(Rz)) + +def is_rotation_matrix(R): + + Rt = numpy.transpose(R) + shouldBeIdentity = numpy.dot(Rt, R) + I = numpy.identity(3, dtype = R.dtype) + n = numpy.linalg.norm(I - shouldBeIdentity) + + return n < 1e-3 + +def make_euler_rotation_vector(R): + + assert(is_rotation_matrix(R)) + + sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) + + singular = sy < 1e-6 + + if not singular : + x = math.atan2(R[2,1] , R[2,2]) + y = math.atan2(-R[2,0], sy) + z = math.atan2(R[1,0], R[0,0]) + else : + x = math.atan2(-R[1,2], R[1,1]) + y = math.atan2(-R[2,0], sy) + z = 0 + + return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)]) + +@dataclass(frozen=True) +class Place(): + """Define a place as a pose and a marker.""" + + translation: numpy.array + """Position in group referential.""" + + rotation: numpy.array + """Rotation in group referential.""" + + marker: dict + """ArUco marker linked to the place.""" + +@dataclass +class ArUcoMarkersGroup(): + """Handle group of ArUco markers as one unique spatial entity and estimate its pose.""" + + marker_size: float = field(default=0.) + """Expected size of all markers in the group.""" + + dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary) + """Expected dictionary of all markers in the group.""" + + places: dict = field(default_factory=dict) + """Expected markers place""" + + def __post_init__(self): + """Init group pose and places pose.""" + + # Init pose data + self._translation = numpy.zeros(3) + self._rotation = numpy.zeros(3) + + # Normalize places data + new_places = {} + + for identifier, data in self.places.items(): + + # Convert string identifier to int value + if type(identifier) == str: + + identifier = int(identifier) + + # Get translation vector + tvec = numpy.array(data.pop('translation')).astype(numpy.float32) + + # Check rotation value shape + rvalue = numpy.array(data.pop('rotation')).astype(numpy.float32) + + # Rotation matrix + if rvalue.shape == (3, 3): + + rmat = rvalue + + # Rotation vector (expected in degree) + elif rvalue.shape == (3,): + + rmat = make_rotation_matrix(rvalue[0], rvalue[1], rvalue[2]).astype(numpy.float32) + + else: + + raise ValueError(f'Bad rotation value: {rvalue}') + + assert(is_rotation_matrix(rmat)) + + new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size) + + new_places[identifier] = Place(tvec, rmat, new_marker) + + # else places are configured using detected markers + elif isinstance(data, ArUcoMarker.ArUcoMarker): + + new_places[identifier] = Place(data.translation, data.rotation, data) + + # else places are already at expected format + elif (type(identifier) == int) and isinstance(data, Place): + + new_places[identifier] = data + + self.places = new_places + + # Init place consistency + self.init_places_consistency() + + @classmethod + def from_obj(self, obj_filepath: str) -> ArUcoMarkersGroupType: + """Load ArUco markers group from .obj file. + + !!! note + Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker + + !!! note + All markers have to belong to the same dictionary. + + !!! note + Marker normal vectors (vn) expected. + + """ + + new_marker_size = 0 + new_dictionary = None + new_places = {} + + # Regex rules for .obj file parsing + OBJ_RX_DICT = { + 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), + 'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), + 'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), + 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n'), + 'comment': re.compile(r'#(.*)\n') # keep comment regex after object regex because the # is used in object string too + } + + # Regex .obj line parser + def __parse_obj_line(line): + + for key, rx in OBJ_RX_DICT.items(): + match = rx.search(line) + if match: + return key, match + + # If there are no matches + return None, None + + # Start parsing + try: + + identifier = None + vertices = [] + normals = {} + faces = {} + + # Open the file and read through it line by line + with open(obj_filepath, 'r') as file: + + line = file.readline() + + while line: + + # At each line check for a match with a regex + key, match = __parse_obj_line(line) + + # Extract comment + if key == 'comment': + pass + + # Extract marker dictionary and identifier + elif key == 'object': + + dictionary = str(match.group(1)) + identifier = int(match.group(2)) + last = str(match.group(3)) + + # Init new group dictionary with first dictionary name + if new_dictionary == None: + + new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) + + # Check all others marker dictionary are equal to new group dictionary + elif dictionary != new_dictionary.name: + + raise NameError(f'Marker {identifier} dictionary is not {new_dictionary.name}') + + # Fill vertices array + elif key == 'vertice': + + vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) + + # Extract normal to calculate rotation matrix + elif key == 'normal': + + normals[identifier] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]) + + # Extract vertice ids + elif key == 'face': + + faces[identifier] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))] + + # Go to next line + line = file.readline() + + file.close() + + # Retreive marker vertices thanks to face vertice ids + for identifier, face in faces.items(): + + # Gather place corners from counter clockwise ordered face vertices + corners = numpy.array([ vertices[i-1] for i in face ]) + + # Edit translation (Tp) allowing to move world axis (W) at place axis (P) + Tp = corners.mean(axis=0) + + # Edit place axis from corners positions + place_x_axis = corners[1:3].mean(axis=0) - Tp + place_x_axis_norm = numpy.linalg.norm(place_x_axis) + place_x_axis = place_x_axis / place_x_axis_norm + + place_y_axis = corners[2:4].mean(axis=0) - Tp + place_y_axis_norm = numpy.linalg.norm(place_y_axis) + place_y_axis = place_y_axis / place_y_axis_norm + + place_z_axis = normals[identifier] + + # Edit rotation (Rp) allowing to transform world axis (W) into place axis (P) + W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) + P = numpy.array([place_x_axis, place_y_axis, place_z_axis]) + Rp = W.dot(P.T) + + # Check axis size: they should be almost equal + if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): + + current_marker_size = place_x_axis_norm*2 + + # Check that all markers size are almost equal + if new_marker_size > 0: + + if not math.isclose(current_marker_size, new_marker_size, rel_tol=1e-3): + + raise ValueError('Markers size should be almost equal.') + + new_marker_size = current_marker_size + + # Create a new place related to a new marker + new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) + new_places[identifier] = Place(Tp, Rp, new_marker) + + except IOError: + raise IOError(f'File not found: {obj_filepath}') + + return ArUcoMarkersGroup(new_marker_size, new_dictionary, new_places) + + @classmethod + def from_json(self, json_filepath: str) -> ArUcoMarkersGroupType: + """Load ArUco markers group from .json file.""" + + new_marker_size = 0 + new_dictionary = None + new_places = {} + + with open(json_filepath) as configuration_file: + + data = json.load(configuration_file) + + new_marker_size = data.pop('marker_size') + new_dictionary = data.pop('dictionary') + new_places = data.pop('places') + + return ArUcoMarkersGroup(new_marker_size, new_dictionary, new_places) + + def __str__(self) -> str: + """String display""" + + output = f'\n\tDictionary: {self.dictionary}' + + output += f'\n\tMarker size: {self.marker_size} cm' + + output += '\n\n\tPlaces:' + for identifier, place in self.places.items(): + output += f'\n\t\t- {identifier}:' + output += f'\n{place.translation}' + output += f'\n{place.rotation}' + + output += '\n\n\tAngle cache:' + for A_identifier, A_angle_cache in self.__rotation_cache.items(): + for B_identifier, angle in A_angle_cache.items(): + output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]' + + output += '\n\n\tDistance cache:' + for A_identifier, A_distance_cache in self.__translation_cache.items(): + for B_identifier, distance in A_distance_cache.items(): + output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}' + + return output + + @property + def identifiers(self) -> list: + """List place marker identifiers belonging to the group.""" + + return list(self.places.keys()) + + def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]: + """Sort markers belonging to the group from given detected markers dict (cf ArUcoDetector.detect_markers()). + + Returns: + dict of markers belonging to this group + dict of remaining markers not belonging to this group + """ + + group_markers = {} + remaining_markers = {} + + for (marker_id, marker) in detected_markers.items(): + + if marker_id in self.places.keys(): + + group_markers[marker_id] = marker + + else: + + remaining_markers[marker_id] = marker + + return group_markers, remaining_markers + + def init_places_consistency(self): + """Initialize places consistency to speed up further markers consistency checking.""" + + # Process expected rotation between places combinations to speed up further calculations + self.__rotation_cache = {} + for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): + + A = self.places[A_identifier].rotation + B = self.places[B_identifier].rotation + + if numpy.array_equal(A, B): + + AB_rvec = [0., 0., 0.] + BA_rvec = [0., 0., 0.] + + else: + + # Calculate euler angle representation of AB and BA rotation matrix + AB_rvec = make_euler_rotation_vector(B.dot(A.T)) + BA_rvec = make_euler_rotation_vector(A.dot(B.T)) + + try: + self.__rotation_cache[A_identifier][B_identifier] = AB_rvec + except: + self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec} + + try: + self.__rotation_cache[B_identifier][A_identifier] = BA_rvec + except: + self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec} + + # Process translation between each places combinations to speed up further calculations + self.__translation_cache = {} + for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): + + A = self.places[A_identifier].translation + B = self.places[B_identifier].translation + + # Calculate translation between A and B position + AB_tvec = numpy.linalg.norm(B - A) + + try: + self.__translation_cache[A_identifier][B_identifier] = AB_tvec + except: + self.__translation_cache[A_identifier] = {B_identifier: AB_tvec} + + try: + self.__translation_cache[B_identifier][A_identifier] = AB_tvec + except: + self.__translation_cache[B_identifier] = {A_identifier: AB_tvec} + + def check_markers_consistency(self, group_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict, dict]: + """Evaluate if given markers configuration match related places configuration. + + Returns: + dict of consistent markers + dict of unconsistent markers + dict of identified distance or angle unconsistencies and out-of-bounds values + """ + + consistent_markers = {} + unconsistencies = {'rotation': {}, 'translation': {}} + + for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(group_markers.items(), 2): + + try: + + # Rotation matrix from A marker to B marker + AB = B_marker.rotation.dot(A_marker.rotation.T) + + # Calculate euler angle representation of AB rotation matrix + AB_rvec = make_euler_rotation_vector(AB) + expected_rvec= self.__rotation_cache[A_identifier][B_identifier] + + # Calculate distance between A marker center and B marker center + AB_tvec = numpy.linalg.norm(A_marker.translation - B_marker.translation) + expected_tvec = self.__translation_cache[A_identifier][B_identifier] + + # Check angle and distance according given tolerance then normalise marker pose + consistent_rotation = numpy.allclose(AB_rvec, expected_rvec, atol=angle_tolerance) + consistent_translation = math.isclose(AB_tvec, expected_tvec, abs_tol=distance_tolerance) + + if consistent_rotation and consistent_translation: + + if A_identifier not in consistent_markers.keys(): + + # Remember this marker is already validated + consistent_markers[A_identifier] = A_marker + + if B_identifier not in consistent_markers.keys(): + + # Remember this marker is already validated + consistent_markers[B_identifier] = B_marker + + else: + + if not consistent_rotation: + unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec} + + if not consistent_translation: + unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec} + + except KeyError: + + raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the group.') + + # Gather unconsistent markers + unconsistent_markers = {} + + for identifier, marker in group_markers.items(): + + if identifier not in consistent_markers.keys(): + + unconsistent_markers[identifier] = marker + + return consistent_markers, unconsistent_markers, unconsistencies + + def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: + """Calculate rotation and translation that move a marker to its place.""" + + # Get the place related to the given marker + try: + + place = self.places[marker.identifier] + + # Rotation matrix that transform marker to related place + self._rotation = marker.rotation.dot(place.rotation.T) + + # Translation vector that transform marker to related place + self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) + + return self._translation, self._rotation + + except KeyError: + + raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') + + def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]: + """Calculate average rotation and translation that move markers to their related places.""" + + rotations = [] + translations = [] + + for identifier, marker in markers.items(): + + try: + + place = self.places[identifier] + + # Rotation matrix that transform marker to related place + R = marker.rotation.dot(place.rotation.T) + + # Translation vector that transform marker to related place + T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) + + rotations.append(R) + translations.append(T) + + except KeyError: + + raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') + + # Consider ArUcoMarkersGroup rotation as the mean of all marker rotations + # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before. + self._rotation = numpy.mean(numpy.array(rotations), axis=0) + + # Consider ArUcoMarkersGroup translation as the mean of all marker translations + self._translation = numpy.mean(numpy.array(translations), axis=0) + + return self._translation, self._rotation + + def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: + """Calculate rotation and translation from 3 markers defining an orthogonal axis.""" + + O_marker = origin_marker + A_marker = horizontal_axis_marker + B_marker = vertical_axis_marker + + O_place = self.places[O_marker.identifier] + A_place = self.places[A_marker.identifier] + B_place = self.places[B_marker.identifier] + + # Place axis + OA = A_place.translation - O_place.translation + OA = OA / numpy.linalg.norm(OA) + + OB = B_place.translation - O_place.translation + OB = OB / numpy.linalg.norm(OB) + + # Detect and correct bad place axis orientation + X_sign = numpy.sign(OA)[0] + Y_sign = numpy.sign(OB)[1] + + P = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) + + # Marker axis + OA = A_marker.translation - O_marker.translation + OA = OA / numpy.linalg.norm(OA) + + OB = B_marker.translation - O_marker.translation + OB = OB / numpy.linalg.norm(OB) + + # Detect and correct bad place axis orientation + X_sign = numpy.sign(OA)[0] + Y_sign = -numpy.sign(OB)[1] + + M = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) + + # Then estimate ArUcoMarkersGroup rotation + self._rotation = P.dot(M.T) + + # Consider ArUcoMarkersGroup translation as the translation of the marker at axis origin + self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T) + + return self._translation, self._rotation + + @property + def translation(self) -> numpy.array: + """Access to group translation vector.""" + + return self._translation + + @translation.setter + def translation(self, tvec): + + self._translation = tvec + + @property + def rotation(self) -> numpy.array: + """Access to group rotation matrix.""" + + return self._translation + + @rotation.setter + def rotation(self, rmat): + + self._rotation = rmat + + def draw_axis(self, image: numpy.array, K, D, consistency=2): + """Draw group axis according a consistency score.""" + + l = self.marker_size / 2 + ll = self.marker_size + + # Select color according consistency score + n = 95 * consistency if consistency < 2 else 0 + f = 159 * consistency if consistency < 2 else 255 + + try: + + # Draw axis + axisPoints = numpy.float32([[ll, 0, 0], [0, ll, 0], [0, 0, ll], [0, 0, 0]]).reshape(-1, 3) + axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + axisPoints = axisPoints.astype(int) + + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red) + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) + + # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations. + except cv.error: + pass + + def draw_places(self, image: numpy.array, K, D, consistency=2): + """Draw group places and their axis according a consistency score.""" + + l = self.marker_size / 2 + ll = self.marker_size + + # Select color according consistency score + n = 95 * consistency if consistency < 2 else 0 + f = 159 * consistency if consistency < 2 else 255 + + for identifier, place in self.places.items(): + + try: + + T = self.places[identifier].translation + R = self.places[identifier].rotation + + # Draw place axis + axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3) + axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + axisPoints = axisPoints.astype(int) + + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red) + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) + cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) + + # Draw place + placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3) + placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + placePoints = placePoints.astype(int) + + cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), (f,f,f), 3) + cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), (f,f,f), 3) + cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), (f,f,f), 3) + cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), (f,f,f), 3) + + # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. + except cv.error: + pass + + def to_obj(self, obj_filepath): + """Save group to .obj file.""" + + with open(obj_filepath, 'w', encoding='utf-8') as file: + + file.write('# ArGaze OBJ File\n') + file.write('# http://achil.recherche.enac.fr/features/eye/argaze/\n') + + v_count = 0 + + for identifier, place in self.places.items(): + + file.write(f'o {self.dictionary.name}#{identifier}_Marker\n') + + vertices = '' + + T = place.translation + R = place.rotation + + points = (T + numpy.float32([R.dot(place.marker.points[0]), R.dot(place.marker.points[1]), R.dot(place.marker.points[2]), R.dot(place.marker.points[3])])).reshape(-1, 3) + + print(points) + + # Write vertices in reverse order + for i in [3, 2, 1, 0]: + + file.write(f'v {" ".join(map(str, points[i]))}\n') + v_count += 1 + + vertices += f' {v_count}' + + file.write('s off\n') + file.write(f'f{vertices}\n') diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 77ddb65..227d3c6 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -""" """ +"""ArScene based of ArUco markers technology.""" __author__ = "Théo de la Hogue" __credits__ = [] @@ -10,708 +10,141 @@ __license__ = "BSD" from typing import TypeVar, Tuple from dataclasses import dataclass, field import json -import math -import itertools -import re +import os -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator +from argaze import ArFeatures, DataStructures +from argaze.ArUcoMarkers import ArUcoMarkersGroup +from argaze.AreaOfInterest import AOI2DScene +import cv2 import numpy -import cv2 as cv -import cv2.aruco as aruco - -T0 = numpy.array([0., 0., 0.]) -"""Define no translation vector.""" - -R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) -"""Define no rotation matrix.""" ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene") # Type definition for type annotation convenience -def make_rotation_matrix(x, y, z): - - # Create rotation matrix around x axis - c = numpy.cos(numpy.deg2rad(x)) - s = numpy.sin(numpy.deg2rad(x)) - Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) - - # Create rotation matrix around y axis - c = numpy.cos(numpy.deg2rad(y)) - s = numpy.sin(numpy.deg2rad(y)) - Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) - - # Create rotation matrix around z axis - c = numpy.cos(numpy.deg2rad(z)) - s = numpy.sin(numpy.deg2rad(z)) - Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - - # Return intrinsic rotation matrix - return Rx.dot(Ry.dot(Rz)) - -def is_rotation_matrix(R): - - Rt = numpy.transpose(R) - shouldBeIdentity = numpy.dot(Rt, R) - I = numpy.identity(3, dtype = R.dtype) - n = numpy.linalg.norm(I - shouldBeIdentity) - - return n < 1e-3 - -def make_euler_rotation_vector(R): - - assert(is_rotation_matrix(R)) - - sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) - - singular = sy < 1e-6 - - if not singular : - x = math.atan2(R[2,1] , R[2,2]) - y = math.atan2(-R[2,0], sy) - z = math.atan2(R[1,0], R[0,0]) - else : - x = math.atan2(-R[1,2], R[1,1]) - y = math.atan2(-R[2,0], sy) - z = 0 - - return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)]) - -@dataclass(frozen=True) -class Place(): - """Define a place as a pose and a marker.""" - - translation: numpy.array - """Position in scene referential.""" - - rotation: numpy.array - """Rotation in scene referential.""" - - marker: dict - """ArUco marker linked to the place.""" - @dataclass -class ArUcoScene(): - """Handle group of ArUco markers as one unique spatial entity and estimate its pose.""" - - marker_size: float = field(default=0.) - """Expected size of all markers in the scene.""" - - dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary) - """Expected dictionary of all markers in the scene.""" - - places: dict = field(default_factory=dict) - """Expected markers place""" - - def __post_init__(self): - """Init scene pose and places pose.""" - - # Init pose data - self._translation = numpy.zeros(3) - self._rotation = numpy.zeros(3) - - # Normalize places data - new_places = {} - - for identifier, data in self.places.items(): - - # Convert string identifier to int value - if type(identifier) == str: - - identifier = int(identifier) - - # Get translation vector - tvec = numpy.array(data.pop('translation')).astype(numpy.float32) - - # Check rotation value shape - rvalue = numpy.array(data.pop('rotation')).astype(numpy.float32) - - # Rotation matrix - if rvalue.shape == (3, 3): - - rmat = rvalue - - # Rotation vector (expected in degree) - elif rvalue.shape == (3,): - - rmat = make_rotation_matrix(rvalue[0], rvalue[1], rvalue[2]).astype(numpy.float32) - - else: - - raise ValueError(f'Bad rotation value: {rvalue}') - - assert(is_rotation_matrix(rmat)) - - new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size) - - new_places[identifier] = Place(tvec, rmat, new_marker) - - # else places are configured using detected markers - elif isinstance(data, ArUcoMarker.ArUcoMarker): - - new_places[identifier] = Place(data.translation, data.rotation, data) - - # else places are already at expected format - elif (type(identifier) == int) and isinstance(data, Place): - - new_places[identifier] = data - - self.places = new_places - - # Init place consistency - self.init_places_consistency() - - @classmethod - def from_obj(self, obj_filepath: str) -> ArUcoSceneType: - """Load ArUco scene from .obj file. - - !!! note - Expected object (o) name format: <DICTIONARY>#<IDENTIFIER>_Marker - - !!! note - All markers have to belong to the same dictionary. - - !!! note - Marker normal vectors (vn) expected. - - """ - - new_marker_size = 0 - new_dictionary = None - new_places = {} - - # Regex rules for .obj file parsing - OBJ_RX_DICT = { - 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), - 'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), - 'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), - 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n'), - 'comment': re.compile(r'#(.*)\n') # keep comment regex after object regex because the # is used in object string too - } - - # Regex .obj line parser - def __parse_obj_line(line): - - for key, rx in OBJ_RX_DICT.items(): - match = rx.search(line) - if match: - return key, match - - # If there are no matches - return None, None - - # Start parsing - try: - - identifier = None - vertices = [] - normals = {} - faces = {} - - # Open the file and read through it line by line - with open(obj_filepath, 'r') as file: - - line = file.readline() - - while line: - - # At each line check for a match with a regex - key, match = __parse_obj_line(line) - - # Extract comment - if key == 'comment': - pass - - # Extract marker dictionary and identifier - elif key == 'object': - - dictionary = str(match.group(1)) - identifier = int(match.group(2)) - last = str(match.group(3)) - - # Init new scene dictionary with first dictionary name - if new_dictionary == None: - - new_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) - - # Check all others marker dictionary are equal to new scene dictionary - elif dictionary != new_dictionary.name: - - raise NameError(f'Marker {identifier} dictionary is not {new_dictionary.name}') - - # Fill vertices array - elif key == 'vertice': - - vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) - - # Extract normal to calculate rotation matrix - elif key == 'normal': - - normals[identifier] = tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]) - - # Extract vertice ids - elif key == 'face': - - faces[identifier] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))] - - # Go to next line - line = file.readline() - - file.close() - - # Retreive marker vertices thanks to face vertice ids - for identifier, face in faces.items(): - - # Gather place corners from counter clockwise ordered face vertices - corners = numpy.array([ vertices[i-1] for i in face ]) - - # Edit translation (Tp) allowing to move world axis (W) at place axis (P) - Tp = corners.mean(axis=0) - - # Edit place axis from corners positions - place_x_axis = corners[1:3].mean(axis=0) - Tp - place_x_axis_norm = numpy.linalg.norm(place_x_axis) - place_x_axis = place_x_axis / place_x_axis_norm - - place_y_axis = corners[2:4].mean(axis=0) - Tp - place_y_axis_norm = numpy.linalg.norm(place_y_axis) - place_y_axis = place_y_axis / place_y_axis_norm - - place_z_axis = normals[identifier] - - # Edit rotation (Rp) allowing to transform world axis (W) into place axis (P) - W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - P = numpy.array([place_x_axis, place_y_axis, place_z_axis]) - Rp = W.dot(P.T) - - # Check axis size: they should be almost equal - if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): - - current_marker_size = place_x_axis_norm*2 - - # Check that all markers size are almost equal - if new_marker_size > 0: - - if not math.isclose(current_marker_size, new_marker_size, rel_tol=1e-3): - - raise ValueError('Markers size should be almost equal.') - - new_marker_size = current_marker_size - - # Create a new place related to a new marker - new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) - new_places[identifier] = Place(Tp, Rp, new_marker) - - except IOError: - raise IOError(f'File not found: {obj_filepath}') - - return ArUcoScene(new_marker_size, new_dictionary, new_places) - - @classmethod - def from_json(self, json_filepath: str) -> ArUcoSceneType: - """Load ArUco scene from .json file.""" - - new_marker_size = 0 - new_dictionary = None - new_places = {} - - with open(json_filepath) as configuration_file: - - data = json.load(configuration_file) - - new_marker_size = data.pop('marker_size') - new_dictionary = data.pop('dictionary') - new_places = data.pop('places') - - return ArUcoScene(new_marker_size, new_dictionary, new_places) - - def __str__(self) -> str: - """String display""" - - output = f'\n\tDictionary: {self.dictionary}' - - output += f'\n\tMarker size: {self.marker_size} cm' - - output += '\n\n\tPlaces:' - for identifier, place in self.places.items(): - output += f'\n\t\t- {identifier}:' - output += f'\n{place.translation}' - output += f'\n{place.rotation}' - - output += '\n\n\tAngle cache:' - for A_identifier, A_angle_cache in self.__rotation_cache.items(): - for B_identifier, angle in A_angle_cache.items(): - output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]' - - output += '\n\n\tDistance cache:' - for A_identifier, A_distance_cache in self.__translation_cache.items(): - for B_identifier, distance in A_distance_cache.items(): - output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}' - - return output - - @property - def identifiers(self) -> list: - """List place marker identifiers belonging to the scene.""" - - return list(self.places.keys()) - - def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]: - """Sort markers belonging to the scene from given detected markers dict (cf ArUcoDetector.detect_markers()). - - Returns: - dict of markers belonging to this scene - dict of remaining markers not belonging to this scene - """ - - scene_markers = {} - remaining_markers = {} - - for (marker_id, marker) in detected_markers.items(): - - if marker_id in self.places.keys(): - - scene_markers[marker_id] = marker - - else: - - remaining_markers[marker_id] = marker - - return scene_markers, remaining_markers - - def init_places_consistency(self): - """Initialize places consistency to speed up further markers consistency checking.""" - - # Process expected rotation between places combinations to speed up further calculations - self.__rotation_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): - - A = self.places[A_identifier].rotation - B = self.places[B_identifier].rotation - - if numpy.array_equal(A, B): - - AB_rvec = [0., 0., 0.] - BA_rvec = [0., 0., 0.] - - else: - - # Calculate euler angle representation of AB and BA rotation matrix - AB_rvec = make_euler_rotation_vector(B.dot(A.T)) - BA_rvec = make_euler_rotation_vector(A.dot(B.T)) - - try: - self.__rotation_cache[A_identifier][B_identifier] = AB_rvec - except: - self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec} - - try: - self.__rotation_cache[B_identifier][A_identifier] = BA_rvec - except: - self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec} - - # Process translation between each places combinations to speed up further calculations - self.__translation_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): - - A = self.places[A_identifier].translation - B = self.places[B_identifier].translation - - # Calculate translation between A and B position - AB_tvec = numpy.linalg.norm(B - A) - - try: - self.__translation_cache[A_identifier][B_identifier] = AB_tvec - except: - self.__translation_cache[A_identifier] = {B_identifier: AB_tvec} - - try: - self.__translation_cache[B_identifier][A_identifier] = AB_tvec - except: - self.__translation_cache[B_identifier] = {A_identifier: AB_tvec} - - def check_markers_consistency(self, scene_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict, dict]: - """Evaluate if given markers configuration match related places configuration. - - Returns: - dict of consistent markers - dict of unconsistent markers - dict of identified distance or angle unconsistencies and out-of-bounds values - """ - - consistent_markers = {} - unconsistencies = {'rotation': {}, 'translation': {}} - - for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(scene_markers.items(), 2): - - try: - - # Rotation matrix from A marker to B marker - AB = B_marker.rotation.dot(A_marker.rotation.T) - - # Calculate euler angle representation of AB rotation matrix - AB_rvec = make_euler_rotation_vector(AB) - expected_rvec= self.__rotation_cache[A_identifier][B_identifier] - - # Calculate distance between A marker center and B marker center - AB_tvec = numpy.linalg.norm(A_marker.translation - B_marker.translation) - expected_tvec = self.__translation_cache[A_identifier][B_identifier] - - # Check angle and distance according given tolerance then normalise marker pose - consistent_rotation = numpy.allclose(AB_rvec, expected_rvec, atol=angle_tolerance) - consistent_translation = math.isclose(AB_tvec, expected_tvec, abs_tol=distance_tolerance) - - if consistent_rotation and consistent_translation: - - if A_identifier not in consistent_markers.keys(): - - # Remember this marker is already validated - consistent_markers[A_identifier] = A_marker - - if B_identifier not in consistent_markers.keys(): - - # Remember this marker is already validated - consistent_markers[B_identifier] = B_marker - - else: - - if not consistent_rotation: - unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec} - - if not consistent_translation: - unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec} - - except KeyError: - - raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the scene.') - - # Gather unconsistent markers - unconsistent_markers = {} - - for identifier, marker in scene_markers.items(): - - if identifier not in consistent_markers.keys(): - - unconsistent_markers[identifier] = marker - - return consistent_markers, unconsistent_markers, unconsistencies - - def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: - """Calculate rotation and translation that move a marker to its place.""" - - # Get the place related to the given marker - try: - - place = self.places[marker.identifier] - - # Rotation matrix that transform marker to related place - self._rotation = marker.rotation.dot(place.rotation.T) - - # Translation vector that transform marker to related place - self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) - - return self._translation, self._rotation - - except KeyError: - - raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.') - - def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]: - """Calculate average rotation and translation that move markers to their related places.""" - - rotations = [] - translations = [] - - for identifier, marker in markers.items(): - - try: - - place = self.places[identifier] - - # Rotation matrix that transform marker to related place - R = marker.rotation.dot(place.rotation.T) - - # Translation vector that transform marker to related place - T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) - - rotations.append(R) - translations.append(T) - - except KeyError: - - raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the scene.') - - # Consider ArUcoScene rotation as the mean of all marker rotations - # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before. - self._rotation = numpy.mean(numpy.array(rotations), axis=0) - - # Consider ArUcoScene translation as the mean of all marker translations - self._translation = numpy.mean(numpy.array(translations), axis=0) - - return self._translation, self._rotation - - def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: - """Calculate rotation and translation from 3 markers defining an orthogonal axis.""" - - O_marker = origin_marker - A_marker = horizontal_axis_marker - B_marker = vertical_axis_marker - - O_place = self.places[O_marker.identifier] - A_place = self.places[A_marker.identifier] - B_place = self.places[B_marker.identifier] - - # Place axis - OA = A_place.translation - O_place.translation - OA = OA / numpy.linalg.norm(OA) - - OB = B_place.translation - O_place.translation - OB = OB / numpy.linalg.norm(OB) - - # Detect and correct bad place axis orientation - X_sign = numpy.sign(OA)[0] - Y_sign = numpy.sign(OB)[1] - - P = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) - - # Marker axis - OA = A_marker.translation - O_marker.translation - OA = OA / numpy.linalg.norm(OA) - - OB = B_marker.translation - O_marker.translation - OB = OB / numpy.linalg.norm(OB) - - # Detect and correct bad place axis orientation - X_sign = numpy.sign(OA)[0] - Y_sign = -numpy.sign(OB)[1] - - M = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) - - # Then estimate ArUcoScene rotation - self._rotation = P.dot(M.T) - - # Consider ArUcoScene translation as the translation of the marker at axis origin - self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T) - - return self._translation, self._rotation - - @property - def translation(self) -> numpy.array: - """Access to scene translation vector.""" - - return self._translation +class ArUcoScene(ArFeatures.ArScene): + """ + Define an ArScene based on an ArUcoMarkersGroup description. - @translation.setter - def translation(self, tvec): + Parameters: + + aruco_markers_group: ArUco markers 3D scene description used to estimate scene pose from detected markers: see [estimate_pose][argaze.ArFeatures.ArScene.estimate_pose] function below. + + """ + aruco_markers_group: ArUcoMarkersGroup.ArUcoMarkersGroup = field(default_factory=ArUcoMarkersGroup.ArUcoMarkersGroup) - self._translation = tvec + def __post_init__(self): - @property - def rotation(self) -> numpy.array: - """Access to scene rotation matrix.""" + super().__post_init__() - return self._translation + def __str__(self) -> str: + """ + Returns: + String representation + """ - @rotation.setter - def rotation(self, rmat): + output = output = super().__str__() + output += f'ArUcoMarkersGroup:\n{self.aruco_markers_group}\n' - self._rotation = rmat + return output - def draw_axis(self, image: numpy.array, K, D, consistency=2): - """Draw scene axis according a consistency score.""" + @classmethod + def from_dict(self, aruco_scene_data: dict, working_directory: str = None) -> ArUcoSceneType: + """ + Load ArUcoScene from dictionary. - l = self.marker_size / 2 - ll = self.marker_size + Parameters: + aruco_scene_data: dictionary + working_directory: folder path where to load files when a dictionary value is a relative filepath. + """ - # Select color according consistency score - n = 95 * consistency if consistency < 2 else 0 - f = 159 * consistency if consistency < 2 else 255 + # Load aruco markers group + try: - try: + # Check aruco_markers_group value type + aruco_markers_group_value = aruco_scene_data.pop('aruco_markers_group') - # Draw axis - axisPoints = numpy.float32([[ll, 0, 0], [0, ll, 0], [0, 0, ll], [0, 0, 0]]).reshape(-1, 3) - axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) - axisPoints = axisPoints.astype(int) + # str: relative path to .obj file + if type(aruco_markers_group_value) == str: - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) + aruco_markers_group_value = os.path.join(working_directory, aruco_markers_group_value) + new_aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup.from_obj(aruco_markers_group_value) - # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations. - except cv.error: - pass + # dict: + else: - def draw_places(self, image: numpy.array, K, D, consistency=2): - """Draw scene places and their axis according a consistency score.""" + new_aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(**aruco_markers_group_value) - l = self.marker_size / 2 - ll = self.marker_size + except KeyError: - # Select color according consistency score - n = 95 * consistency if consistency < 2 else 0 - f = 159 * consistency if consistency < 2 else 255 + new_aruco_markers_group = None - for identifier, place in self.places.items(): + # Get values of temporary ar scene created from aruco_scene_data + temp_ar_scene_values = DataStructures.as_dict(ArFeatures.ArScene.from_dict(aruco_scene_data, working_directory)) - try: + # Create new aruco scene using temporary ar scene values + return ArUcoScene(aruco_markers_group=new_aruco_markers_group, **temp_ar_scene_values) + + def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]: + """Estimate scene pose from detected ArUco markers. - T = self.places[identifier].translation - R = self.places[identifier].rotation + Returns: + scene translation vector + scene rotation matrix + pose estimation strategy + dict of markers used to estimate the pose + """ - # Draw place axis - axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3) - axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) - axisPoints = axisPoints.astype(int) + # Pose estimation fails when no marker is detected + if len(detected_markers) == 0: - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (n,n,f), 6) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (n,f,n), 6) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (f,n,n), 6) # Z (blue) - - # Draw place - placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3) - placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) - placePoints = placePoints.astype(int) - - cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), (f,f,f), 3) - cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), (f,f,f), 3) + raise PoseEstimationFailed('No marker detected') - # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. - except cv.error: - pass + scene_markers, _ = self.aruco_markers_group.filter_markers(detected_markers) - def to_obj(self, obj_filepath): - """Save ArUco scene to .obj file.""" + # Pose estimation fails when no marker belongs to the scene + if len(scene_markers) == 0: - with open(obj_filepath, 'w', encoding='utf-8') as file: + raise PoseEstimationFailed('No marker belongs to the scene') - file.write('# ArGaze OBJ File\n') - file.write('# http://achil.recherche.enac.fr/features/eye/argaze/\n') + # Estimate scene pose from unique marker transformations + elif len(scene_markers) == 1: - v_count = 0 + marker_id, marker = scene_markers.popitem() + tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(marker) + + return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker} - for identifier, place in self.places.items(): + # Otherwise, check markers consistency + consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance) - file.write(f'o {self.dictionary.name}#{identifier}_Marker\n') + # Pose estimation fails when no marker passes consistency checking + if len(consistent_markers) == 0: - vertices = '' + raise PoseEstimationFailed('Unconsistent marker poses', unconsistencies) - T = place.translation - R = place.rotation + # Otherwise, estimate scene pose from all consistent markers pose + tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(consistent_markers) - points = (T + numpy.float32([R.dot(place.marker.points[0]), R.dot(place.marker.points[1]), R.dot(place.marker.points[2]), R.dot(place.marker.points[3])])).reshape(-1, 3) + return tvec, rmat, 'estimate_pose_from_markers', consistent_markers - print(points) + def draw_axis(self, image: numpy.array): + """ + Draw scene axis into image. + + Parameters: + image: where to draw + """ - # Write vertices in reverse order - for i in [3, 2, 1, 0]: + self.aruco_markers_group.draw_axis(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D) - file.write(f'v {" ".join(map(str, points[i]))}\n') - v_count += 1 + def draw_places(self, image: numpy.array): + """ + Draw scene places into image. - vertices += f' {v_count}' + Parameters: + image: where to draw + """ - file.write('s off\n') - file.write(f'f{vertices}\n') + self.aruco_markers_group.draw_places(image, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D) diff --git a/src/argaze/ArUcoMarkers/__init__.py b/src/argaze/ArUcoMarkers/__init__.py index 350c69e..0ca48cc 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', 'ArUcoOpticCalibrator', 'ArUcoDetector', 'ArUcoScene', 'utils']
\ No newline at end of file +__all__ = ['ArUcoMarkersDictionary', 'ArUcoMarker', 'ArUcoBoard', 'ArUcoOpticCalibrator', 'ArUcoDetector', 'ArUcoMarkersGroup', 'ArUcoCamera', 'ArUcoScene', 'utils']
\ No newline at end of file diff --git a/src/argaze/utils/aruco_markers_scene_export.py b/src/argaze/utils/aruco_markers_scene_export.py index 4518e48..c1a0991 100644 --- a/src/argaze/utils/aruco_markers_scene_export.py +++ b/src/argaze/utils/aruco_markers_scene_export.py @@ -11,7 +11,7 @@ import argparse import time import itertools -from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoOpticCalibrator, ArUcoDetector, ArUcoScene +from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoOpticCalibrator, ArUcoDetector, ArUcoMarkersGroup from argaze.utils import MiscFeatures import cv2 @@ -54,7 +54,7 @@ def main(): aruco_detector = ArUcoDetector.ArUcoDetector(dictionary=aruco_dictionary, marker_size=args.marker_size, optic_parameters=optic_parameters, parameters=detector_parameters) # Create empty ArUco scene - aruco_scene = None + aruco_markers_group = None # Create a window to display AR environment window_name = "Export ArUco scene" @@ -96,7 +96,7 @@ def main(): aruco_detector.estimate_markers_pose() # Build aruco scene from detected markers - aruco_scene = ArUcoScene.ArUcoScene(args.marker_size, aruco_dictionary, aruco_detector.detected_markers) + aruco_markers_group = ArUcoMarkersGroup.ArUcoMarkersGroup(args.marker_size, aruco_dictionary, aruco_detector.detected_markers) # Write scene detected markers cv2.putText(video_image, f'{list(aruco_detector.detected_markers.keys())}', (20, image_height-80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) @@ -149,9 +149,9 @@ def main(): # Save selected marker edition using 'Ctrl + s' if key_pressed == 19: - if aruco_scene: + if aruco_markers_group: - aruco_scene.to_obj(f'{args.output}/{int(current_image_time)}-aruco_scene.obj') + aruco_markers_group.to_obj(f'{args.output}/{int(current_image_time)}-aruco_markers_group.obj') print(f'ArUco scene saved into {args.output}') else: diff --git a/src/argaze/utils/demo_augmented_reality_run.py b/src/argaze/utils/demo_augmented_reality_run.py index 25d4083..14ddd36 100644 --- a/src/argaze/utils/demo_augmented_reality_run.py +++ b/src/argaze/utils/demo_augmented_reality_run.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -""" """ +"""Augmented Reality pipeline demo script.""" __author__ = "Théo de la Hogue" __credits__ = [] @@ -13,28 +13,29 @@ import os import time from argaze import ArFeatures, GazeFeatures +from argaze.ArUcoMarkers import ArUcoCamera import cv2 import numpy def main(): """ - Load AR environment from .json file, detect ArUco markers into camera device images and project it. + Load ArUcoCamera from .json file, detect ArUco markers into camera device images and project it. """ current_directory = os.path.dirname(os.path.abspath(__file__)) # Manage arguments parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0]) - parser.add_argument('environment', metavar='ENVIRONMENT', type=str, help='ar environment filepath') + parser.add_argument('aruco_camera', metavar='ARUCO_CAMERA', type=str, help='ArUco camera filepath') parser.add_argument('-s', '--source', metavar='SOURCE', type=str, default='0', help='video capture source (a number to select camera device or a filepath to load a movie)') args = parser.parse_args() - # Load AR enviroment - ar_environment = ArFeatures.ArEnvironment.from_json(args.environment) + # Load ArUcoCamera + aruco_camera = ArUcoCamera.ArUcoCamera.from_json(args.aruco_camera) - # Create a window to display AR environment - cv2.namedWindow(ar_environment.name, cv2.WINDOW_AUTOSIZE) + # Create a window to display ArUcoCamera + cv2.namedWindow(aruco_camera.name, cv2.WINDOW_AUTOSIZE) # Init timestamp start_time = time.time() @@ -45,17 +46,17 @@ def main(): # Edit millisecond timestamp timestamp = int((time.time() - start_time) * 1e3) - # Project gaze position into environment - for frame, look_data in ar_environment.look(timestamp, GazeFeatures.GazePosition((x, y))): + # Project gaze position into camera + for frame, look_data in aruco_camera.look(timestamp, GazeFeatures.GazePosition((x, y))): # Unpack look data - movement, scan_step_analysis, layer_analysis, execution_times, exception = look_data + gaze_movement, scan_step_analysis, layer_analysis, execution_times, exception = look_data # Do something with look data # ... # Attach mouse callback to window - cv2.setMouseCallback(ar_environment.name, on_mouse_event) + cv2.setMouseCallback(aruco_camera.name, on_mouse_event) # Enable camera video capture into separate thread video_capture = cv2.VideoCapture(int(args.source) if args.source.isdecimal() else args.source) @@ -71,28 +72,28 @@ def main(): if success: - # Detect and project environment - detection_time, exceptions = ar_environment.detect_and_project(video_image) + # Detect and project AR features + detection_time, exceptions = aruco_camera.detect_and_project(video_image) - # Get environment image - environment_image = ar_environment.image() + # Get ArUcoCamera image + aruco_camera_image = aruco_camera.image() # Write detection fps - cv2.rectangle(environment_image, (0, 0), (420, 50), (63, 63, 63), -1) - cv2.putText(environment_image, f'Detection fps: {1e3/detection_time:.1f}', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) + cv2.rectangle(aruco_camera_image, (0, 0), (420, 50), (63, 63, 63), -1) + cv2.putText(aruco_camera_image, f'Detection fps: {1e3/detection_time:.1f}', (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA) # Handle exceptions for i, (scene_name, e) in enumerate(exceptions.items()): # Write errors - cv2.rectangle(environment_image, (0, (i+1)*50), (720, (i+2)*50), (127, 127, 127), -1) - cv2.putText(environment_image, f'{scene_name} error: {e}', (20, (i+1)*90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) + cv2.rectangle(aruco_camera_image, (0, (i+1)*50), (720, (i+2)*50), (127, 127, 127), -1) + cv2.putText(aruco_camera_image, f'{scene_name} error: {e}', (20, (i+1)*90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA) - # Display environment - cv2.imshow(ar_environment.name, environment_image) + # Display ArUcoCamera image + cv2.imshow(aruco_camera.name, aruco_camera_image) # Draw and display each frames - for frame in ar_environment.frames: + for frame in aruco_camera.frames: # Display frame cv2.imshow(f'{frame.parent.name}:{frame.name}', frame.image()) diff --git a/src/argaze/utils/demo_environment/aoi_3d_scene.obj b/src/argaze/utils/demo_environment/aoi_3d_scene.obj index 8922e78..d32e235 100644 --- a/src/argaze/utils/demo_environment/aoi_3d_scene.obj +++ b/src/argaze/utils/demo_environment/aoi_3d_scene.obj @@ -1,4 +1,4 @@ -# Blender v3.0.1 OBJ File: 'ar_environment.blend' +# Blender v3.0.1 OBJ File: 'ar_camera.blend' # www.blender.org o GrayRectangle v 0.000000 0.000000 0.000000 diff --git a/src/argaze/utils/demo_environment/aruco_scene.obj b/src/argaze/utils/demo_environment/aruco_markers_group.obj index 9ad43be..1030d01 100644 --- a/src/argaze/utils/demo_environment/aruco_scene.obj +++ b/src/argaze/utils/demo_environment/aruco_markers_group.obj @@ -1,4 +1,4 @@ -# Blender v3.0.1 OBJ File: 'ar_environment.blend' +# Blender v3.0.1 OBJ File: 'ar_camera.blend' # www.blender.org o DICT_APRILTAG_16h5#0_Marker v -5.000000 14.960000 0.000000 diff --git a/src/argaze/utils/demo_environment/demo_augmented_reality_setup.json b/src/argaze/utils/demo_environment/demo_augmented_reality_setup.json index b1c0696..f157120 100644 --- a/src/argaze/utils/demo_environment/demo_augmented_reality_setup.json +++ b/src/argaze/utils/demo_environment/demo_augmented_reality_setup.json @@ -1,5 +1,6 @@ { - "name": "ArEnvironment Demo", + "name": "ArUcoCamera Demo", + "size": [1280, 720], "aruco_detector": { "dictionary": { "name": "DICT_APRILTAG_16h5" @@ -12,33 +13,31 @@ "aprilTagDeglitch": 1 } }, - "camera_frame": { - "layers": { - "Camera_layer": {} - }, - "image_parameters": { - "background_weight": 1, - "draw_layers": { - "Camera_layer": { - "draw_aoi_scene": { - "draw_aoi": { - "color": [255, 255, 255], - "border_size": 1 - } + "layers": { + "main_layer": {} + }, + "image_parameters": { + "background_weight": 1, + "draw_layers": { + "main_layer": { + "draw_aoi_scene": { + "draw_aoi": { + "color": [255, 255, 255], + "border_size": 1 } } - }, - "draw_gaze_position": { - "color": [0, 255, 255], - "size": 4 } + }, + "draw_gaze_position": { + "color": [0, 255, 255], + "size": 4 } }, "scenes": { "ArScene Demo" : { - "aruco_scene": "aruco_scene.obj", + "aruco_markers_group": "aruco_markers_group.obj", "layers": { - "Camera_layer" : { + "main_layer" : { "aoi_scene": "aoi_3d_scene.obj" } }, @@ -111,49 +110,6 @@ } } }, - "aruco_axis": { - "lower_left_corner": { - "origin_marker": 2, - "horizontal_axis_marker": 3, - "vertical_axis_marker": 0 - }, - "lower_right_corner": { - "origin_marker": 3, - "horizontal_axis_marker": 2, - "vertical_axis_marker": 1 - }, - "upper_left_corner": { - "origin_marker": 0, - "horizontal_axis_marker": 1, - "vertical_axis_marker": 2 - }, - "upper_right_corner": { - "origin_marker": 1, - "horizontal_axis_marker": 0, - "vertical_axis_marker": 3 - } - }, - "aruco_aoi": { - "GrayRectangle": { - "upper_left_corner": { - "marker_identifier": 0, - "marker_corner_index": 2 - }, - "upper_right_corner": { - "marker_identifier": 1, - "marker_corner_index": 3 - }, - "lower_left_corner": { - "marker_identifier": 2, - "marker_corner_index": 1 - }, - "lower_right_corner": { - "marker_identifier": 3, - "marker_corner_index": 0 - }, - "inner_aoi": "all" - } - }, "angle_tolerance": 15.0, "distance_tolerance": 2.54 } diff --git a/src/argaze/utils/demo_gaze_analysis_run.py b/src/argaze/utils/demo_gaze_analysis_run.py index 92fa282..465c5db 100644 --- a/src/argaze/utils/demo_gaze_analysis_run.py +++ b/src/argaze/utils/demo_gaze_analysis_run.py @@ -34,7 +34,7 @@ def main(): # Load ArFrame ar_frame = ArFeatures.ArFrame.from_json(args.frame) - # Create a window to display ArEnvironment + # Create a window to display ArCamera cv2.namedWindow(ar_frame.name, cv2.WINDOW_AUTOSIZE) # Heatmap buffer display option |